public static void write(shape_msgs.msg.dds.Mesh data, us.ihmc.idl.CDR cdr) { if(data.getTriangles().size() <= 100) cdr.write_type_e(data.getTriangles());else throw new RuntimeException("triangles field exceeds the maximum length"); if(data.getVertices().size() <= 100) cdr.write_type_e(data.getVertices());else throw new RuntimeException("vertices field exceeds the maximum length"); }
public static void write(tf2_msgs.msg.dds.TFMessage data, us.ihmc.idl.CDR cdr) { if(data.getTransforms().size() <= 100) cdr.write_type_e(data.getTransforms());else throw new RuntimeException("transforms field exceeds the maximum length"); }
public static void write(rcl_interfaces.msg.dds.ParameterEvent data, us.ihmc.idl.CDR cdr) { if(data.getNewParameters().size() <= 100) cdr.write_type_e(data.getNewParameters());else throw new RuntimeException("new_parameters field exceeds the maximum length"); if(data.getChangedParameters().size() <= 100) cdr.write_type_e(data.getChangedParameters());else throw new RuntimeException("changed_parameters field exceeds the maximum length"); if(data.getDeletedParameters().size() <= 100) cdr.write_type_e(data.getDeletedParameters());else throw new RuntimeException("deleted_parameters field exceeds the maximum length"); }
public static void write(controller_msgs.msg.dds.FootstepPlanRequestPacket data, us.ihmc.idl.CDR cdr) { cdr.write_type_4(data.getSequenceId()); controller_msgs.msg.dds.FootstepDataMessagePubSubType.write(data.getStartFootstep(), cdr); cdr.write_type_6(data.getThetaStart()); cdr.write_type_6(data.getMaxSubOptimality()); if(data.getGoals().size() <= 100) cdr.write_type_e(data.getGoals());else throw new RuntimeException("goals field exceeds the maximum length"); cdr.write_type_9(data.getFootstepPlanRequestType()); }
public final static int getCdrSerializedSize(controller_msgs.msg.dds.FootstepNodeDataListMessage data, int current_alignment) { int initial_alignment = current_alignment; current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); for(int i0 = 0; i0 < data.getNodeData().size(); ++i0) { current_alignment += controller_msgs.msg.dds.FootstepNodeDataMessagePubSubType.getCdrSerializedSize(data.getNodeData().get(i0), current_alignment);} current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); return current_alignment - initial_alignment; }
public int setUpForwardTiltedBlockTest() throws SimulationExceededMaximumTimeException { OffsetAndYawRobotInitialSetup startingLocation = new OffsetAndYawRobotInitialSetup(3.5, 0.0, 0.0); PlanarRegionsListMessage planarRegionsListMessage = setUpTest(startingLocation); FootstepDataListMessage footsteps = createTiltedBlocksForwardFootstepDataListMessage(swingTime, transferTime); drcSimulationTestHelper.publishToController(footsteps); drcSimulationTestHelper.publishToController(planarRegionsListMessage); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(1.0); return footsteps.getFootstepDataList().size(); }
private void addJointStates(Handshake handshake) { for (int i = 0; i < handshake.getJoints().size(); i++) { JointDefinition joint = handshake.getJoints().get(i); jointStates.add(JointState.createJointState(joint.getNameAsString(), joint.getType())); } }
public static FrameConvexPolygon2D unpackFootSupportPolygon(CapturabilityBasedStatus capturabilityBasedStatus, RobotSide robotSide) { if (robotSide == RobotSide.LEFT && capturabilityBasedStatus.getLeftFootSupportPolygon2d().size() > 0) return new FrameConvexPolygon2D(ReferenceFrame.getWorldFrame(), Vertex3DSupplier.asVertex3DSupplier(capturabilityBasedStatus.getLeftFootSupportPolygon2d())); else if (capturabilityBasedStatus.getRightFootSupportPolygon2d() != null) return new FrameConvexPolygon2D(ReferenceFrame.getWorldFrame(), Vertex3DSupplier.asVertex3DSupplier(capturabilityBasedStatus.getRightFootSupportPolygon2d())); else return new FrameConvexPolygon2D(ReferenceFrame.getWorldFrame()); }
public void set(FootstepDataListMessage footStepList) { outgoingFootstepDataList = footStepList; numberOfFootsteps.set(outgoingFootstepDataList.getFootstepDataList().size()); packetHasBeenSent.set(false); }
public final static int getCdrSerializedSize(nav_msgs.msg.dds.Path data, int current_alignment) { int initial_alignment = current_alignment; current_alignment += std_msgs.msg.dds.HeaderPubSubType.getCdrSerializedSize(data.getHeader(), current_alignment); current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); for(int i0 = 0; i0 < data.getPoses().size(); ++i0) { current_alignment += geometry_msgs.msg.dds.PoseStampedPubSubType.getCdrSerializedSize(data.getPoses().get(i0), current_alignment);} return current_alignment - initial_alignment; }
public final static int getCdrSerializedSize(visualization_msgs.msg.dds.InteractiveMarkerInit data, int current_alignment) { int initial_alignment = current_alignment; current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + data.getServerId().length() + 1; current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); for(int i0 = 0; i0 < data.getMarkers().size(); ++i0) { current_alignment += visualization_msgs.msg.dds.InteractiveMarkerPubSubType.getCdrSerializedSize(data.getMarkers().get(i0), current_alignment);} return current_alignment - initial_alignment; }
public final static int getCdrSerializedSize(test_msgs.msg.dds.DynamicArrayPrimitivesNested data, int current_alignment) { int initial_alignment = current_alignment; current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); for(int i0 = 0; i0 < data.getDynamicArrayPrimitiveValues().size(); ++i0) { current_alignment += test_msgs.msg.dds.DynamicArrayPrimitivesPubSubType.getCdrSerializedSize(data.getDynamicArrayPrimitiveValues().get(i0), current_alignment);} return current_alignment - initial_alignment; }
public static void write(nav_msgs.msg.dds.GridCells data, us.ihmc.idl.CDR cdr) { std_msgs.msg.dds.HeaderPubSubType.write(data.getHeader(), cdr); cdr.write_type_5(data.getCellWidth()); cdr.write_type_5(data.getCellHeight()); if(data.getCells().size() <= 100) cdr.write_type_e(data.getCells());else throw new RuntimeException("cells field exceeds the maximum length"); }
public static void write(controller_msgs.msg.dds.BodyPathPlanStatisticsMessage data, us.ihmc.idl.CDR cdr) { cdr.write_type_4(data.getSequenceId()); cdr.write_type_2(data.getPlanId()); if(data.getNavigableRegions().size() <= 25) cdr.write_type_e(data.getNavigableRegions());else throw new RuntimeException("navigable_regions field exceeds the maximum length"); controller_msgs.msg.dds.VisibilityMapMessagePubSubType.write(data.getInterRegionsMap(), cdr); controller_msgs.msg.dds.VisibilityMapMessagePubSubType.write(data.getStartVisibilityMap(), cdr); controller_msgs.msg.dds.VisibilityMapMessagePubSubType.write(data.getGoalVisibilityMap(), cdr); }
public int setUpTiltedBlockTest() throws SimulationExceededMaximumTimeException { OffsetAndYawRobotInitialSetup startingLocation = new OffsetAndYawRobotInitialSetup(3.5, 0.0, 0.0); PlanarRegionsListMessage planarRegionsListMessage = setUpTest(startingLocation); FootstepDataListMessage footsteps = createTiltedBlocksFootstepDataListMessage(swingTime, transferTime); drcSimulationTestHelper.publishToController(footsteps); drcSimulationTestHelper.publishToController(planarRegionsListMessage); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(1.0); return footsteps.getFootstepDataList().size(); }
private void addTimeDelay(MomentumTrajectoryMessage momentumTrajectoryMessage, double timeDelay) { EuclideanTrajectoryMessage angularMomentumTrajectory = momentumTrajectoryMessage.getAngularMomentumTrajectory(); Object<EuclideanTrajectoryPointMessage> taskspaceTrajectoryPoints = angularMomentumTrajectory.getTaskspaceTrajectoryPoints(); for (int i = 0; i < taskspaceTrajectoryPoints.size(); i++) { EuclideanTrajectoryPointMessage trajectoryPoint = taskspaceTrajectoryPoints.get(i); trajectoryPoint.setTime(trajectoryPoint.getTime() + timeDelay); } }
private static Graphics3DObject createTrajectoryMessageVisualization(WaypointBasedTrajectoryMessage trajectoryMessage, double radius, AppearanceDefinition appearance) { double t0 = trajectoryMessage.getWaypointTimes().get(0); double tf = trajectoryMessage.getWaypointTimes().get(trajectoryMessage.getWaypoints().size() - 1); double timeResolution = (tf - t0) / trajectoryMessage.getWaypoints().size(); FunctionTrajectory trajectoryToVisualize = WholeBodyTrajectoryToolboxMessageTools.createFunctionTrajectory(trajectoryMessage); return createFunctionTrajectoryVisualization(trajectoryToVisualize, t0, tf, timeResolution, radius, appearance); }
public final static int getCdrSerializedSize(controller_msgs.msg.dds.Polygon2DMessage data, int current_alignment) { int initial_alignment = current_alignment; current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); for(int i0 = 0; i0 < data.getVertices().size(); ++i0) { current_alignment += geometry_msgs.msg.dds.PointPubSubType.getCdrSerializedSize(data.getVertices().get(i0), current_alignment);} return current_alignment - initial_alignment; }
public static void write(controller_msgs.msg.dds.OneDoFJointTrajectoryMessage data, us.ihmc.idl.CDR cdr) { cdr.write_type_4(data.getSequenceId()); if(data.getTrajectoryPoints().size() <= 50) cdr.write_type_e(data.getTrajectoryPoints());else throw new RuntimeException("trajectory_points field exceeds the maximum length"); cdr.write_type_6(data.getWeight()); }
public static void write(controller_msgs.msg.dds.KinematicsPlanningToolboxOutputStatus data, us.ihmc.idl.CDR cdr) { cdr.write_type_4(data.getSequenceId()); if(data.getKeyFrameTimes().size() <= 100) cdr.write_type_e(data.getKeyFrameTimes());else throw new RuntimeException("key_frame_times field exceeds the maximum length"); if(data.getRobotConfigurations().size() <= 100) cdr.write_type_e(data.getRobotConfigurations());else throw new RuntimeException("robot_configurations field exceeds the maximum length"); cdr.write_type_6(data.getSolutionQuality()); }