public FootstepDataCommand() { clear(); }
public void addFootstep(FootstepDataCommand footstep) { footsteps.add().set(footstep); }
this.robotSide = command.getRobotSide(); this.swingTrajectoryBlendDuration = command.getSwingTrajectoryBlendDuration(); this.trajectoryType = command.getTrajectoryType(); this.swingHeight = command.getSwingHeight(); this.isAdjustable = isAdjustable; this.trustHeight = trustHeight; this.footstepPose.setIncludingFrame(command.getPosition(), command.getOrientation()); RecyclingArrayList<Point2D> commandPredictedContactPoints = command.getPredictedContactPoints(); if (commandPredictedContactPoints != null) RecyclingArrayList<FramePoint3D> commandCustomPositionWaypoints = command.getCustomPositionWaypoints(); if (commandCustomPositionWaypoints != null) RecyclingArrayList<FrameSE3TrajectoryPoint> commandSwingTrajectory = command.getSwingTrajectory(); if (commandSwingTrajectory != null)
private Footstep createFootstep(FootstepDataCommand footstepData) FramePose footstepPose = new FramePose(worldFrame, footstepData.getPosition(), footstepData.getOrientation()); PoseReferenceFrame footstepPoseFrame = new PoseReferenceFrame("footstepPoseFrame", footstepPose); if (footstepData.getPredictedContactPoints().isEmpty()) contactPoints = null; else for (int i = 0; i < footstepData.getPredictedContactPoints().size(); i++) contactPoints.add(footstepData.getPredictedContactPoints().get(i)); RobotSide robotSide = footstepData.getRobotSide(); TrajectoryType trajectoryType = footstepData.getTrajectoryType(); if (trajectoryType == TrajectoryType.CUSTOM) if (footstepData.getTrajectoryWaypoints() == null) RecyclingArrayList<Point3d> trajectoryWaypoints = footstepData.getTrajectoryWaypoints(); footstep.setSwingWaypoints(trajectoryWaypoints); footstep.setSwingHeight(footstepData.getSwingHeight()); switch (footstepData.getOrigin())
public void createStepPlan() desiredFootstepCommand.clear(); footstepCommandList.clear(); desiredFootstepCommand.clear(); desiredFootstepCommand.setOrigin(FootstepOrigin.AT_SOLE_FRAME); desiredFootstepCommand.setRobotSide(swingSide); desiredFootstepCommand.setSwingHeight(swingHeight.getDoubleValue());
FootstepDataCommand footstepCommand = new FootstepDataCommand(); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.LEFT); footstepList.addFootstep(footstepCommand); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.RIGHT); footstepList.addFootstep(footstepCommand); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.LEFT); footstepList.addFootstep(footstepCommand); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.RIGHT); footstepList.addFootstep(footstepCommand); footstepCommand = new FootstepDataCommand(); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.LEFT); footstepList.addFootstep(footstepCommand); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.RIGHT); footstepList.addFootstep(footstepCommand); footstepCommand.setPose(position, orientation);
desiredFootstepCommand.setPose(desiredPosition.getPoint(), desiredOrientation.getQuaternion()); desiredFootstepCommand.setPredictedContactPoints(contactPoints);
FootstepDataCommand footstepCommand = new FootstepDataCommand(); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.LEFT); footstepList.addFootstep(footstepCommand); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.RIGHT); footstepList.addFootstep(footstepCommand); footstepCommand = new FootstepDataCommand(); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.LEFT); footstepList.addFootstep(footstepCommand); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.RIGHT); footstepList.addFootstep(footstepCommand); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.LEFT); footstepList.addFootstep(footstepCommand); footstepCommand.setPose(position, orientation); footstepCommand.setRobotSide(RobotSide.RIGHT); footstepList.addFootstep(footstepCommand);
public void addFootstep(FootstepDataCommand footstep) { footsteps.add().set(footstep); }
public FootstepDataCommand() { clear(); }
@Override public void set(FootstepDataListMessage message) { clear(); defaultSwingTime = message.defaultSwingTime; defaultTransferTime = message.defaultTransferTime; finalTransferTime = message.finalTransferTime; executionMode = message.executionMode; ArrayList<FootstepDataMessage> dataList = message.getDataList(); if (dataList != null) { for (int i = 0; i < dataList.size(); i++) footsteps.add().set(dataList.get(i)); } }
@Override public void set(FootstepDataListCommand other) { clear(); defaultSwingTime = other.defaultSwingTime; defaultTransferTime = other.defaultTransferTime; finalTransferTime = other.finalTransferTime; executionMode = other.executionMode; RecyclingArrayList<FootstepDataCommand> otherFootsteps = other.getFootsteps(); if (otherFootsteps != null) { for (int i = 0; i < otherFootsteps.size(); i++) footsteps.add().set(otherFootsteps.get(i)); } }
@Override public void set(FootstepDataListCommand other) { clear(); defaultSwingDuration = other.defaultSwingDuration; defaultTransferDuration = other.defaultTransferDuration; finalTransferDuration = other.finalTransferDuration; executionTiming = other.executionTiming; adjustedExecutionTime = other.adjustedExecutionTime; trustHeightOfFootsteps = other.trustHeightOfFootsteps; areFootstepsAdjustable = other.areFootstepsAdjustable; offsetFootstepsWithExecutionError = other.offsetFootstepsWithExecutionError; RecyclingArrayList<FootstepDataCommand> otherFootsteps = other.getFootsteps(); if (otherFootsteps != null) { for (int i = 0; i < otherFootsteps.size(); i++) footsteps.add().set(otherFootsteps.get(i)); } setQueueableCommandVariables(other); }
@Override public void setFromMessage(FootstepDataListMessage message) { clear(); defaultSwingDuration = message.getDefaultSwingDuration(); defaultTransferDuration = message.getDefaultTransferDuration(); finalTransferDuration = message.getFinalTransferDuration(); executionTiming = ExecutionTiming.fromByte(message.getExecutionTiming()); trustHeightOfFootsteps = message.getTrustHeightOfFootsteps(); areFootstepsAdjustable = message.getAreFootstepsAdjustable(); offsetFootstepsWithExecutionError = message.getOffsetFootstepsWithExecutionError(); List<FootstepDataMessage> dataList = message.getFootstepDataList(); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); if (dataList != null) { for (int i = 0; i < dataList.size(); i++) footsteps.add().set(worldFrame, dataList.get(i)); } setQueueableCommandVariables(message.getQueueingProperties()); }