public static String validateQuadrupedBodyHeightMessage(QuadrupedBodyHeightMessage message) { String errorMessage = validatePacket(message); if (errorMessage != null) return message.getClass().getSimpleName() + " " + errorMessage; EuclideanTrajectoryPointMessage previousTrajectoryPoint = null; if (message.getEuclideanTrajectory().getTaskspaceTrajectoryPoints().isEmpty()) { String messageClassName = message.getClass().getSimpleName(); errorMessage = "Received " + messageClassName + " with no waypoint."; return errorMessage; } for (int i = 0; i < message.getEuclideanTrajectory().getTaskspaceTrajectoryPoints().size(); i++) { EuclideanTrajectoryPointMessage waypoint = message.getEuclideanTrajectory().getTaskspaceTrajectoryPoints().get(i); errorMessage = validateEuclideanTrajectoryPointMessage(waypoint, previousTrajectoryPoint, false); if (errorMessage != null) { String messageClassName = message.getClass().getSimpleName(); errorMessage = "The " + messageClassName + "'s " + i + "th waypoint " + errorMessage; return errorMessage; } previousTrajectoryPoint = waypoint; } return null; }
public static String validatePelvisHeightTrajectoryMessage(PelvisHeightTrajectoryMessage message) { String errorMessage = validatePacket(message); if (errorMessage != null) return message.getClass().getSimpleName() + " " + errorMessage; EuclideanTrajectoryPointMessage previousTrajectoryPoint = null; if (message.getEuclideanTrajectory().getTaskspaceTrajectoryPoints().isEmpty()) { String messageClassName = message.getClass().getSimpleName(); errorMessage = "Received " + messageClassName + " with no waypoint."; return errorMessage; } for (int i = 0; i < message.getEuclideanTrajectory().getTaskspaceTrajectoryPoints().size(); i++) { EuclideanTrajectoryPointMessage waypoint = message.getEuclideanTrajectory().getTaskspaceTrajectoryPoints().get(i); errorMessage = validateEuclideanTrajectoryPointMessage(waypoint, previousTrajectoryPoint, false); if (errorMessage != null) { String messageClassName = message.getClass().getSimpleName(); errorMessage = "The " + messageClassName + "'s " + i + "th waypoint " + errorMessage; return errorMessage; } previousTrajectoryPoint = waypoint; } return null; }
if (message.getPositionTrajectory().getTaskspaceTrajectoryPoints().isEmpty())
return errorMessage; if (!message.getLeftHandTrajectoryMessage().getSe3Trajectory().getTaskspaceTrajectoryPoints().isEmpty()) return "The robotSide of leftHandTrajectoryMessage field is inconsistent with its name."; if (!message.getLeftHandTrajectoryMessage().getSe3Trajectory().getTaskspaceTrajectoryPoints().isEmpty()) return "The robotSide of rightHandTrajectoryMessage field is inconsistent with its name."; if (!message.getLeftArmTrajectoryMessage().getJointspaceTrajectory().getJointTrajectoryMessages().isEmpty()) return "The robotSide of leftArmTrajectoryMessage field is inconsistent with its name."; if (!message.getRightArmTrajectoryMessage().getJointspaceTrajectory().getJointTrajectoryMessages().isEmpty()) return "The robotSide of rightArmTrajectoryMessage field is inconsistent with its name."; if (!message.getChestTrajectoryMessage().getSo3Trajectory().getTaskspaceTrajectoryPoints().isEmpty()) if (!message.getPelvisTrajectoryMessage().getSe3Trajectory().getTaskspaceTrajectoryPoints().isEmpty()) if (!message.getHeadTrajectoryMessage().getSo3Trajectory().getTaskspaceTrajectoryPoints().isEmpty()) if (!message.getLeftFootTrajectoryMessage().getSe3Trajectory().getTaskspaceTrajectoryPoints().isEmpty()) if (!message.getRightFootTrajectoryMessage().getSe3Trajectory().getTaskspaceTrajectoryPoints().isEmpty())
ROS2Tools.createCallbackSubscription(ros2Node, CapturabilityBasedStatus.class, controllerPubGenerator, s -> { CapturabilityBasedStatus status = s.takeNextData(); isLeftFootInSupport.set(!status.getLeftFootSupportPolygon2d().isEmpty()); isRightFootInSupport.set(!status.getRightFootSupportPolygon2d().isEmpty()); });
if (!nodeData.isEmpty()) broadcastNodeData(nodeDataListMessage);
@Override public double getFootstepValue(FootstepDataMessage footstepData) { RotationMatrix rotationMatrix = new RotationMatrix(); rotationMatrix.set(footstepData.getOrientation()); Vector3D footstepNormal = new Vector3D(); rotationMatrix.getColumn(2, footstepNormal); double offHorizontalAngle = Math.acos(footstepNormal.getZ()); if (offHorizontalAngle > parameters.getMaxAngle() || footstepNormal.getZ() < 0) return Double.NEGATIVE_INFINITY; double value = slopeGain * offHorizontalAngle; if (footstepData.getPredictedContactPoints2d() == null || footstepData.getPredictedContactPoints2d().isEmpty()) return Double.NEGATIVE_INFINITY; ConvexPolygon2D supportPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(HumanoidMessageTools.unpackPredictedContactPoints(footstepData))); supportPolygon.update(); double inPlaneArea = supportPolygon.getArea(); double horizonalArea = inPlaneArea * footstepNormal.getZ(); if (horizonalArea < parameters.getMinArea()) return Double.NEGATIVE_INFINITY; value += horizonalArea * areaGain; return value; }
private static FootstepPlan convertToFootstepPlan(FootstepDataListMessage footstepDataListMessage) { FootstepPlan footstepPlan = new FootstepPlan(); for (FootstepDataMessage footstepMessage : footstepDataListMessage.getFootstepDataList()) { FramePose3D stepPose = new FramePose3D(); stepPose.setPosition(footstepMessage.getLocation()); stepPose.setOrientation(footstepMessage.getOrientation()); SimpleFootstep simpleFootstep = footstepPlan.addFootstep(RobotSide.fromByte(footstepMessage.getRobotSide()), stepPose); IDLSequence.Object<Point3D> predictedContactPoints = footstepMessage.getPredictedContactPoints2d(); if (!predictedContactPoints.isEmpty()) { ConvexPolygon2D foothold = new ConvexPolygon2D(); for (int i = 0; i < predictedContactPoints.size(); i++) { foothold.addVertex(predictedContactPoints.get(i)); } foothold.update(); simpleFootstep.setFoothold(foothold); } } return footstepPlan; } }
public static HeightQuadTree convertMessage(HeightQuadTreeMessage messageToConvert) { HeightQuadTree heightQuadTree = new HeightQuadTree(); heightQuadTree.setDefaultHeight(messageToConvert.getDefaultHeight()); heightQuadTree.setResolution(messageToConvert.getResolution()); heightQuadTree.setSizeX(messageToConvert.getSizeX()); heightQuadTree.setSizeY(messageToConvert.getSizeY()); if (messageToConvert.getLeaves().isEmpty()) return heightQuadTree; HeightQuadTreeNode root = new HeightQuadTreeNode(); root.setCenterX(0.0f); root.setCenterY(0.0f); root.setSizeX(messageToConvert.getSizeX()); root.setSizeY(messageToConvert.getSizeY()); for (int i = 0; i < messageToConvert.getLeaves().size(); i++) insertLeafRecursive(root, messageToConvert.getLeaves().get(i)); heightQuadTree.setRoot(root); return heightQuadTree; }
public static double unpackTrajectoryTime(JointspaceTrajectoryMessage jointspaceTrajectoryMessage) { double trajectoryTime = 0.0; for (int i = 0; i < jointspaceTrajectoryMessage.getJointTrajectoryMessages().size(); i++) { OneDoFJointTrajectoryMessage oneDoFJointTrajectoryMessage = jointspaceTrajectoryMessage.getJointTrajectoryMessages().get(i); if (oneDoFJointTrajectoryMessage != null && !oneDoFJointTrajectoryMessage.getTrajectoryPoints().isEmpty()) { trajectoryTime = Math.max(trajectoryTime, oneDoFJointTrajectoryMessage.getTrajectoryPoints().getLast().getTime()); } } return trajectoryTime; } }