public void setFootstepList(FootstepDataListMessage footMessage) { this.footstepListMessage = footMessage; this.footstepList = new ArrayList<>(); for (int i = 0; i < this.footstepListMessage.getFootstepDataList().size(); i++) footstepList.add(this.footstepListMessage.getFootstepDataList().get(i)); } }
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())); } }
private void addTimeCorruption(MomentumTrajectoryMessage momentumTrajectoryMessage) { EuclideanTrajectoryMessage angularMomentumTrajectory = momentumTrajectoryMessage.getAngularMomentumTrajectory(); Object<EuclideanTrajectoryPointMessage> taskspaceTrajectoryPoints = angularMomentumTrajectory.getTaskspaceTrajectoryPoints(); for (int i = 0; i < taskspaceTrajectoryPoints.size(); i++) { EuclideanTrajectoryPointMessage trajectoryPoint = taskspaceTrajectoryPoints.get(i); double timeDeviation = EuclidCoreRandomTools.nextDouble(random, 0.25 * angularMomentumRecordDT); trajectoryPoint.setTime(trajectoryPoint.getTime() + timeDeviation); } }
public final static int getCdrSerializedSize(controller_msgs.msg.dds.NavigableRegionMessage data, int current_alignment) { int initial_alignment = current_alignment; current_alignment += controller_msgs.msg.dds.PlanarRegionMessagePubSubType.getCdrSerializedSize(data.getHomeRegion(), current_alignment); current_alignment += controller_msgs.msg.dds.VisibilityClusterMessagePubSubType.getCdrSerializedSize(data.getHomeRegionCluster(), current_alignment); current_alignment += controller_msgs.msg.dds.VisibilityMapMessagePubSubType.getCdrSerializedSize(data.getVisibilityMapInWorld(), current_alignment); current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); for(int i0 = 0; i0 < data.getObstacleClusters().size(); ++i0) { current_alignment += controller_msgs.msg.dds.VisibilityClusterMessagePubSubType.getCdrSerializedSize(data.getObstacleClusters().get(i0), current_alignment);} return current_alignment - initial_alignment; }
private static void assertDesiredsMatchAfterExecution(SpineTrajectoryMessage message, OneDoFJointBasics[] spineJoints, SimulationConstructionSet scs) { for (int jointIdx = 0; jointIdx < spineJoints.length; jointIdx++) { OneDoFJointTrajectoryMessage jointTrajectory = message.getJointspaceTrajectory().getJointTrajectoryMessages().get(jointIdx); double desired = jointTrajectory.getTrajectoryPoints().getLast().getPosition(); OneDoFJointBasics joint = spineJoints[jointIdx]; assertJointDesired(scs, joint, desired); } }
public final static int getCdrSerializedSize(controller_msgs.msg.dds.QuadrupedBodyPathPlanMessage data, int current_alignment) { int initial_alignment = current_alignment; current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); for(int i0 = 0; i0 < data.getBodyPathPoints().size(); ++i0) { current_alignment += controller_msgs.msg.dds.EuclideanTrajectoryPointMessagePubSubType.getCdrSerializedSize(data.getBodyPathPoints().get(i0), current_alignment);} return current_alignment - initial_alignment; }
public final static int getCdrSerializedSize(sensor_msgs.msg.dds.JoyFeedbackArray 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.getArray().size(); ++i0) { current_alignment += sensor_msgs.msg.dds.JoyFeedbackPubSubType.getCdrSerializedSize(data.getArray().get(i0), current_alignment);} return current_alignment - initial_alignment; }
public final static int getCdrSerializedSize(visualization_msgs.msg.dds.MarkerArray 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.getMarkers().size(); ++i0) { current_alignment += visualization_msgs.msg.dds.MarkerPubSubType.getCdrSerializedSize(data.getMarkers().get(i0), current_alignment);} return current_alignment - initial_alignment; }
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 FootstepDataListMessage corruptDataList(FootstepDataListMessage footstepDataList) { FootstepDataListMessage ret = HumanoidMessageTools.createFootstepDataListMessage(footstepDataList.getDefaultSwingDuration(), footstepDataList.getDefaultTransferDuration()); for (int i = 0; i < footstepDataList.getFootstepDataList().size(); i++) { ret.getFootstepDataList().add().set(corruptFootstepData(footstepDataList.getFootstepDataList().get(i))); } return ret; }
public final static int getCdrSerializedSize(actionlib_msgs.msg.dds.GoalStatusArray 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.getStatusList().size(); ++i0) { current_alignment += actionlib_msgs.msg.dds.GoalStatusPubSubType.getCdrSerializedSize(data.getStatusList().get(i0), current_alignment);} return current_alignment - initial_alignment; }
public final static int getCdrSerializedSize(diagnostic_msgs.msg.dds.DiagnosticArray 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.getStatus().size(); ++i0) { current_alignment += diagnostic_msgs.msg.dds.DiagnosticStatusPubSubType.getCdrSerializedSize(data.getStatus().get(i0), current_alignment);} return current_alignment - initial_alignment; }
public final static int getCdrSerializedSize(test_msgs.msg.dds.DynamicArrayNested 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.getPrimitiveValues().size(); ++i0) { current_alignment += test_msgs.msg.dds.PrimitivesPubSubType.getCdrSerializedSize(data.getPrimitiveValues().get(i0), current_alignment);} return current_alignment - initial_alignment; }
public final static int getCdrSerializedSize(nav_msgs.msg.dds.GridCells 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); 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.getCells().size(); ++i0) { current_alignment += geometry_msgs.msg.dds.PointPubSubType.getCdrSerializedSize(data.getCells().get(i0), current_alignment);} return current_alignment - initial_alignment; }
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; }
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); } }
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; }