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; } }
FootstepNodeDataListMessage packLowestCostPlanMessage() { if (nodeData.isEmpty()) return null; Object<FootstepNodeDataMessage> nodeDataListForMessage = nodeDataListMessage.getNodeData(); nodeDataListForMessage.clear(); List<FootstepNodeDataMessage> nodeData = this.nodeData.getCopyForReading(); for (int i = 0; i < nodeData.size(); i++) nodeDataListForMessage.add().set(nodeData.get(i)); nodeDataListMessage.setIsFootstepGraph(false); hasNodeData.set(false); return nodeDataListMessage; }
public ImageMarker() { header_ = new std_msgs.msg.dds.Header(); ns_ = new java.lang.StringBuilder(255); position_ = new us.ihmc.euclid.tuple3D.Point3D(); outline_color_ = new std_msgs.msg.dds.ColorRGBA(); fill_color_ = new std_msgs.msg.dds.ColorRGBA(); lifetime_ = new builtin_interfaces.msg.dds.Duration(); points_ = new us.ihmc.idl.IDLSequence.Object<us.ihmc.euclid.tuple3D.Point3D> (100, new geometry_msgs.msg.dds.PointPubSubType()); outline_colors_ = new us.ihmc.idl.IDLSequence.Object<std_msgs.msg.dds.ColorRGBA> (100, new std_msgs.msg.dds.ColorRGBAPubSubType()); }
private void executeMessage(SpineTrajectoryMessage message) throws SimulationExceededMaximumTimeException { double controllerDT = getRobotModel().getControllerDT(); drcSimulationTestHelper.publishToController(message); double trajectoryTime = 0.0; for (int jointIdx = 0; jointIdx < numberOfJoints; jointIdx++) { OneDoFJointTrajectoryMessage jointTrajectory = message.getJointspaceTrajectory().getJointTrajectoryMessages().get(jointIdx); double jointTrajectoryTime = jointTrajectory.getTrajectoryPoints().getLast().getTime(); if (jointTrajectoryTime > trajectoryTime) trajectoryTime = jointTrajectoryTime; } assertTrue(drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(trajectoryTime + 5.0 * controllerDT)); assertDesiredsMatchAfterExecution(message, spineJoints, drcSimulationTestHelper.getSimulationConstructionSet()); }
private void computeForwardFootstepList() { forwardFootstepList.getFootstepDataList().clear(); RobotSide swingSide = initialSwingSide.getEnumValue(); for (int i = 0; i < numberOfStepsToTake.getIntegerValue(); i++) { FootstepDataMessage footstepDataMessage = constructFootstepDataMessage(midFootZUpFrame, footstepLength.getDoubleValue() * (i + 1), 0.5 * swingSide.negateIfRightSide(footstepWidth.getDoubleValue()), swingSide); forwardFootstepList.getFootstepDataList().add().set(footstepDataMessage); swingSide = swingSide.getOppositeSide(); } forwardFootstepList.setDefaultSwingDuration(swingTime.getDoubleValue()); forwardFootstepList.setDefaultTransferDuration(transferTime.getDoubleValue()); }
public static SE3TrajectoryMessage createSE3TrajectoryMessage(double trajectoryTime, Point3DReadOnly desiredPosition, QuaternionReadOnly desiredOrientation, long trajectoryReferenceFrameId) { SE3TrajectoryMessage message = new SE3TrajectoryMessage(); Vector3D zeroLinearVelocity = new Vector3D(); Vector3D zeroAngularVelocity = new Vector3D(); message.getTaskspaceTrajectoryPoints().add() .set(createSE3TrajectoryPointMessage(trajectoryTime, desiredPosition, desiredOrientation, zeroLinearVelocity, zeroAngularVelocity)); message.getFrameInformation().setTrajectoryReferenceFrameId(trajectoryReferenceFrameId); return message; }
/** * Use this constructor to go straight to the given end point. * * @param trajectoryTime how long it takes to reach the desired position. * @param desiredPosition desired end point position. */ public static OneDoFJointTrajectoryMessage createOneDoFJointTrajectoryMessage(double trajectoryTime, double desiredPosition) { OneDoFJointTrajectoryMessage message = new OneDoFJointTrajectoryMessage(); message.getTrajectoryPoints().add().set(HumanoidMessageTools.createTrajectoryPoint1DMessage(trajectoryTime, desiredPosition, 0.0)); return message; }
public static NavigableRegionMessage convertToNavigableRegionMessage(NavigableRegion navigableRegion) { NavigableRegionMessage message = new NavigableRegionMessage(); message.getHomeRegion().set(PlanarRegionMessageConverter.convertToPlanarRegionMessage(navigableRegion.getHomeRegion())); message.getHomeRegionCluster().set(convertToVisibilityClusterMessage(navigableRegion.getHomeRegionCluster())); message.getVisibilityMapInWorld().set(convertToVisibilityMapMessage(navigableRegion.getMapId(), navigableRegion.getVisibilityMapInWorld())); List<Cluster> obstacleClusters = navigableRegion.getObstacleClusters(); for (int i = 0; i < obstacleClusters.size(); i++) message.getObstacleClusters().add().set(convertToVisibilityClusterMessage(obstacleClusters.get(i))); return message; }
public void updateFootSupportState(boolean isLeftFootInSupport, boolean isRightFootInSupport) { CapturabilityBasedStatus capturabilityBasedStatus = new CapturabilityBasedStatus(); if (isLeftFootInSupport) capturabilityBasedStatus.getLeftFootSupportPolygon2d().add(); if (isRightFootInSupport) capturabilityBasedStatus.getRightFootSupportPolygon2d().add(); updateCapturabilityBasedStatus(capturabilityBasedStatus); }
private void sendFootPosePacketToController() { if (!isPaused.getBooleanValue() && !isAborted.getBooleanValue()) { outgoingFootTrajectoryMessage.setDestination(PacketDestination.UI.ordinal()); publisher.publish(outgoingFootTrajectoryMessage); hasPacketBeenSent.set(true); trajectoryTime.set(outgoingFootTrajectoryMessage.getSe3Trajectory().getTaskspaceTrajectoryPoints().getLast().getTime()); } }
public static JointspaceTrajectoryMessage createJointspaceTrajectoryMessage(double[] trajectoryTimes, double[] desiredJointPositions) { JointspaceTrajectoryMessage message = new JointspaceTrajectoryMessage(); for (int jointIndex = 0; jointIndex < desiredJointPositions.length; jointIndex++) message.getJointTrajectoryMessages().add().set(createOneDoFJointTrajectoryMessage(trajectoryTimes[jointIndex], desiredJointPositions[jointIndex])); return message; }
public static AdjustFootstepMessage nextAdjustFootstepMessage(Random random) { AdjustFootstepMessage next = new AdjustFootstepMessage(); next.setRobotSide(RandomNumbers.nextEnum(random, RobotSide.class).toByte()); next.getLocation().set(EuclidCoreRandomTools.nextPoint3D(random)); next.getOrientation().set(EuclidCoreRandomTools.nextQuaternion(random)); IntStream.range(0, random.nextInt(10)).mapToObj(i -> EuclidCoreRandomTools.nextPoint2D(random)).forEach(next.getPredictedContactPoints2d().add()::set); next.setExecutionDelayTime(RandomNumbers.nextDoubleWithEdgeCases(random, 0.1)); return next; }
public final static int getCdrSerializedSize(controller_msgs.msg.dds.OneDoFJointTrajectoryMessage 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.getTrajectoryPoints().size(); ++i0) { current_alignment += controller_msgs.msg.dds.TrajectoryPoint1DMessagePubSubType.getCdrSerializedSize(data.getTrajectoryPoints().get(i0), current_alignment);} current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); return current_alignment - initial_alignment; }
private int getNumberOfJointStateVariables(Handshake handshake) { int numberOfJointStates = 0; for (int i = 0; i < handshake.getJoints().size(); i++) { JointDefinition joint = handshake.getJoints().get(i); numberOfJointStates += JointState.getNumberOfVariables(joint.getType()); } return numberOfJointStates; }
public static int getNumberOfVariables(Handshake handShake) { int jointStateVariables = 0; for (int i = 0; i < handShake.getJoints().size(); i++) { jointStateVariables += JointState.getNumberOfVariables(handShake.getJoints().get(i).getType()); } return 1 + handShake.getVariables().size() + jointStateVariables; }
private static FootstepDataListMessage convertFootstepsToFootstepData(ArrayList<Footstep> footsteps, double swingTime, double transferTime) { FootstepDataListMessage footstepsData = HumanoidMessageTools.createFootstepDataListMessage(swingTime, transferTime); for (Footstep footstep : footsteps) { footstepsData.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(footstep)); } return footstepsData; }
private void addFootstep(Point3D stepLocation, Quaternion orient, RobotSide robotSide, FootstepDataListMessage message) { FootstepDataMessage footstepData = new FootstepDataMessage(); footstepData.getLocation().set(stepLocation); footstepData.getOrientation().set(orient); footstepData.setRobotSide(robotSide.toByte()); message.getFootstepDataList().add().set(footstepData); }
private void addFootstep(Point3D stepLocation, Quaternion orient, RobotSide robotSide, FootstepDataListMessage message) { FootstepDataMessage footstepData = new FootstepDataMessage(); footstepData.getLocation().set(stepLocation); footstepData.getOrientation().set(orient); footstepData.setRobotSide(robotSide.toByte()); message.getFootstepDataList().add().set(footstepData); }
public static BodyPathPlanStatisticsMessage convertToBodyPathPlanStatisticsMessage(int planId, VisibilityGraphStatistics statistics) { BodyPathPlanStatisticsMessage message = new BodyPathPlanStatisticsMessage(); message.setPlanId(planId); message.getGoalVisibilityMap().set(convertToVisibilityMapMessage(statistics.getGoalMapId(), statistics.getGoalVisibilityMap())); message.getStartVisibilityMap().set(convertToVisibilityMapMessage(statistics.getStartMapId(), statistics.getStartVisibilityMap())); message.getInterRegionsMap().set(convertToVisibilityMapMessage(statistics.getInterRegionsMapId(), statistics.getInterRegionsVisibilityMap())); for (int i = 0; i < statistics.getNumberOfNavigableRegions(); i++) message.getNavigableRegions().add().set(convertToNavigableRegionMessage(statistics.getNavigableRegion(i))); return message; }
private void createRootRegistry(String rootRegistryName) { YoRegistryDefinition yoRegistryDescription = handshake.getRegistries().add(); yoRegistryDescription.setName(rootRegistryName); yoRegistryDescription.setParent((short) 0); }