public static IntrinsicParametersMessage nextIntrinsicParametersMessage(Random random) { IntrinsicParametersMessage next = new IntrinsicParametersMessage(); next.setWidth(random.nextInt()); next.setHeight(random.nextInt()); next.setFx(random.nextDouble()); next.setFy(random.nextDouble()); next.setSkew(random.nextDouble()); next.setCx(random.nextDouble()); next.setCy(random.nextDouble()); next.getRadial().add(RandomNumbers.nextDoubleArray(random, random.nextInt(1000), 1.0)); next.setT1(random.nextDouble()); next.setT2(random.nextDouble()); return next; }
private void setOutputStatus(WholeBodyTrajectoryToolboxOutputStatus outputStatusToPack, List<SpatialNode> path) { if (outputStatusToPack.getPlanningResult() == 4) { MessageTools.copyData(path.stream().map(SpatialNode::getConfiguration).toArray(size -> new KinematicsToolboxOutputStatus[size]), outputStatusToPack.getRobotConfigurations()); outputStatusToPack.getTrajectoryTimes().reset(); outputStatusToPack.getTrajectoryTimes().add(path.stream().mapToDouble(SpatialNode::getTime).toArray()); //if (VERBOSE) // for (int i = 0; i < path.size(); i++) // PrintTools.info("FINAL RESULT IS " + i + " " + path.get(i).isValid()); } else { if (VERBOSE) PrintTools.info("Planning has Failed."); } }
public static KinematicsPlanningToolboxCenterOfMassMessage createKinematicsPlanningToolboxCenterOfMassMessage(TDoubleArrayList keyFrameTimes, List<Point3DReadOnly> keyFramePoints) { KinematicsPlanningToolboxCenterOfMassMessage message = new KinematicsPlanningToolboxCenterOfMassMessage(); if (keyFrameTimes.size() != keyFramePoints.size()) throw new RuntimeException("Inconsistent list lengths: keyFrameTimes.size() = " + keyFrameTimes.size() + ", keyFramePoints.size() = " + keyFramePoints.size()); for (int i = 0; i < keyFrameTimes.size(); i++) { message.getWayPointTimes().add(keyFrameTimes.get(i)); message.getDesiredWayPointPositionsInWorld().add().set(keyFramePoints.get(i)); } return message; }
objectMessage.getConstants().add(d);
public static WaypointBasedTrajectoryMessage createWaypointBasedTrajectoryMessage(RigidBodyBasics endEffector, double[] waypointTimes, Pose3D[] waypoints, SelectionMatrix6D selectionMatrix) { WaypointBasedTrajectoryMessage message = new WaypointBasedTrajectoryMessage(); message.setEndEffectorHashCode(endEffector.hashCode()); if (waypointTimes.length != waypoints.length) throw new RuntimeException("Inconsistent array lengths."); message.getWaypointTimes().reset(); message.getWaypointTimes().add(waypointTimes); MessageTools.copyData(waypoints, message.getWaypoints()); if (selectionMatrix != null) { message.getAngularSelectionMatrix().setSelectionFrameId(MessageTools.toFrameId(selectionMatrix.getAngularSelectionFrame())); message.getAngularSelectionMatrix().setXSelected(selectionMatrix.isAngularXSelected()); message.getAngularSelectionMatrix().setYSelected(selectionMatrix.isAngularYSelected()); message.getAngularSelectionMatrix().setZSelected(selectionMatrix.isAngularZSelected()); message.getLinearSelectionMatrix().setSelectionFrameId(MessageTools.toFrameId(selectionMatrix.getLinearSelectionFrame())); message.getLinearSelectionMatrix().setXSelected(selectionMatrix.isLinearXSelected()); message.getLinearSelectionMatrix().setYSelected(selectionMatrix.isLinearYSelected()); message.getLinearSelectionMatrix().setZSelected(selectionMatrix.isLinearZSelected()); } return message; }
public static KinematicsPlanningToolboxRigidBodyMessage createKinematicsPlanningToolboxRigidBodyMessage(RigidBodyBasics endEffector, ReferenceFrame controlFrame, TDoubleArrayList keyFrameTimes, List<Pose3DReadOnly> keyFramePoses) { KinematicsPlanningToolboxRigidBodyMessage message = new KinematicsPlanningToolboxRigidBodyMessage(); message.setEndEffectorHashCode(endEffector.hashCode()); RigidBodyTransform transformToBodyFixedFrame = new RigidBodyTransform(); controlFrame.getTransformToDesiredFrame(transformToBodyFixedFrame, endEffector.getBodyFixedFrame()); message.getControlFramePositionInEndEffector().set(transformToBodyFixedFrame.getTranslationVector()); message.getControlFrameOrientationInEndEffector().set(transformToBodyFixedFrame.getRotationMatrix()); if (keyFrameTimes.size() != keyFramePoses.size()) throw new RuntimeException("Inconsistent list lengths: keyFrameTimes.size() = " + keyFrameTimes.size() + ", keyFramePoses.size() = " + keyFramePoses.size()); for (int i = 0; i < keyFrameTimes.size(); i++) { message.getKeyFrameTimes().add(keyFrameTimes.get(i)); message.getKeyFramePoses().add().set(keyFramePoses.get(i)); } KinematicsPlanningToolboxMessageFactory.setDefaultAllowableDisplacement(message, keyFrameTimes.size()); return message; }
public static RigidBodyExplorationConfigurationMessage createRigidBodyExplorationConfigurationMessage(RigidBodyBasics rigidBody, ConfigurationSpaceName[] degreesOfFreedomToExplore, double[] explorationRangeUpperLimits, double[] explorationRangeLowerLimits) { RigidBodyExplorationConfigurationMessage message = new RigidBodyExplorationConfigurationMessage(); if (degreesOfFreedomToExplore.length != explorationRangeUpperLimits.length || degreesOfFreedomToExplore.length != explorationRangeLowerLimits.length) throw new RuntimeException("Inconsistent array lengths: unconstrainedDegreesOfFreedom.length = " + degreesOfFreedomToExplore.length); message.setRigidBodyHashCode(rigidBody.hashCode()); byte[] degreesOfFreedomToExplore1 = ConfigurationSpaceName.toBytes(degreesOfFreedomToExplore); if (degreesOfFreedomToExplore1.length != explorationRangeUpperLimits.length || degreesOfFreedomToExplore1.length != explorationRangeLowerLimits.length) throw new RuntimeException("Inconsistent array lengths: unconstrainedDegreesOfFreedom.length = " + degreesOfFreedomToExplore1.length + ", explorationRangeLowerLimits.length = "); message.getConfigurationSpaceNamesToExplore().reset(); message.getExplorationRangeUpperLimits().reset(); message.getExplorationRangeLowerLimits().reset(); message.getConfigurationSpaceNamesToExplore().add(degreesOfFreedomToExplore1); message.getExplorationRangeUpperLimits().add(explorationRangeUpperLimits); message.getExplorationRangeLowerLimits().add(explorationRangeLowerLimits); return message; }
public void write() { if (fingers == null) { return; } HandJointAnglePacket packet = packetCopier.getCopyForWriting(); if (packet == null) { return; } packet.setRobotSide(side.toByte()); packet.setConnected(connected.get()); packet.setCalibrated(calibrated.get()); packet.getJointAngles().reset(); packet.getJointAngles().add(fingers); packetCopier.commit(); }
public static KinematicsPlanningToolboxRigidBodyMessage createKinematicsPlanningToolboxRigidBodyMessage(RigidBodyBasics endEffector, TDoubleArrayList keyFrameTimes, List<Pose3DReadOnly> keyFramePoses) { KinematicsPlanningToolboxRigidBodyMessage message = new KinematicsPlanningToolboxRigidBodyMessage(); message.setEndEffectorHashCode(endEffector.hashCode()); if (keyFrameTimes.size() != keyFramePoses.size()) throw new RuntimeException("Inconsistent list lengths: keyFrameTimes.size() = " + keyFrameTimes.size() + ", keyFramePoses.size() = " + keyFramePoses.size()); for (int i = 0; i < keyFrameTimes.size(); i++) { message.getKeyFrameTimes().add(keyFrameTimes.get(i)); message.getKeyFramePoses().add().set(keyFramePoses.get(i)); } KinematicsPlanningToolboxMessageFactory.setDefaultAllowableDisplacement(message, keyFrameTimes.size()); return message; }
public static RigidBodyExplorationConfigurationMessage createRigidBodyExplorationConfigurationMessage(RigidBodyBasics rigidBody, ConfigurationSpaceName[] degreesOfFreedomToExplore, double[] explorationRangeAmplitudes) { RigidBodyExplorationConfigurationMessage message = new RigidBodyExplorationConfigurationMessage(); if (degreesOfFreedomToExplore.length != explorationRangeAmplitudes.length) throw new RuntimeException("Inconsistent array lengths: unconstrainedDegreesOfFreedom.length = " + degreesOfFreedomToExplore.length); message.setRigidBodyHashCode(rigidBody.hashCode()); byte[] degreesOfFreedomToExplore1 = ConfigurationSpaceName.toBytes(degreesOfFreedomToExplore); if (degreesOfFreedomToExplore1.length != explorationRangeAmplitudes.length) throw new RuntimeException("Inconsistent array lengths: unconstrainedDegreesOfFreedom.length = " + degreesOfFreedomToExplore1.length + ", explorationRangeLowerLimits.length = "); message.getConfigurationSpaceNamesToExplore().reset(); message.getExplorationRangeUpperLimits().reset(); message.getExplorationRangeLowerLimits().reset(); message.getConfigurationSpaceNamesToExplore().add(degreesOfFreedomToExplore1); for (int i = 0; i < degreesOfFreedomToExplore1.length; i++) { message.getExplorationRangeUpperLimits().add(explorationRangeAmplitudes[i]); message.getExplorationRangeLowerLimits().add(-explorationRangeAmplitudes[i]); } return message; }
public static IntrinsicParametersMessage toIntrinsicParametersMessage(IntrinsicParameters intrinsicParameters) { IntrinsicParametersMessage intrinsicParametersMessage = new IntrinsicParametersMessage(); intrinsicParametersMessage.setWidth(intrinsicParameters.width); intrinsicParametersMessage.setHeight(intrinsicParameters.height); intrinsicParametersMessage.setFx(intrinsicParameters.fx); intrinsicParametersMessage.setFy(intrinsicParameters.fy); intrinsicParametersMessage.setSkew(intrinsicParameters.skew); intrinsicParametersMessage.setCx(intrinsicParameters.cx); intrinsicParametersMessage.setCy(intrinsicParameters.cy); if (intrinsicParameters.radial != null) intrinsicParametersMessage.getRadial().add(intrinsicParameters.radial); intrinsicParametersMessage.setT1(intrinsicParameters.t1); intrinsicParametersMessage.setT2(intrinsicParameters.t2); return intrinsicParametersMessage; }
@Override public void doControl() { for (RobotSide robotSide : RobotSide.values) { packet.setRobotSide(robotSide.toByte()); packet.getJointAngles().reset(); for (ValkyrieHandJointName jointEnum : ValkyrieHandJointName.values) { double q = handJoints.get(robotSide).get(jointEnum).getQ(); packet.getJointAngles().add(q); } publisher.publish(packet); } }
@Override public void readElement(int i, CDR cdr) { add(cdr.read_type_6()); }
public static void setDefaultAllowableDisplacement(KinematicsPlanningToolboxRigidBodyMessage message, int numberOfKeyFrames) { for (int i = 0; i < numberOfKeyFrames; i++) { message.getAllowablePositionDisplacement().add(DEFAULT_POSITION_DISPLACEMENT); message.getAllowableOrientationDisplacement().add(DEFAULT_ORIENTATION_DISPLACEMENT); } }
public static DesiredAccelerationsMessage createDesiredAccelerationsMessage(double[] desiredJointAccelerations) { DesiredAccelerationsMessage message = new DesiredAccelerationsMessage(); message.getDesiredJointAccelerations().add(desiredJointAccelerations); return message; }
public void set(Double other) { resetQuick(); for(int i = 0; i < other.size(); i++) { add(other.get(i)); } }
public static HandJointAnglePacket createHandJointAnglePacket(RobotSide robotSide, boolean connected, boolean calibrated, double[] jointAngles) { HandJointAnglePacket message = new HandJointAnglePacket(); message.setRobotSide(robotSide == null ? -1 : robotSide.toByte()); message.getJointAngles().add(jointAngles); message.setConnected(connected); message.setCalibrated(calibrated); return message; }
public static void packManifold(byte[] configurationSpaces, double[] lowerLimits, double[] upperLimits, ReachingManifoldMessage reachingManifoldMessage) { if (configurationSpaces.length != lowerLimits.length || configurationSpaces.length != upperLimits.length || lowerLimits.length != upperLimits.length) throw new RuntimeException("Inconsistent array lengths: configurationSpaces = " + configurationSpaces.length); reachingManifoldMessage.getManifoldConfigurationSpaceNames().reset(); reachingManifoldMessage.getManifoldLowerLimits().reset(); reachingManifoldMessage.getManifoldUpperLimits().reset(); reachingManifoldMessage.getManifoldConfigurationSpaceNames().add(configurationSpaces); reachingManifoldMessage.getManifoldLowerLimits().add(lowerLimits); reachingManifoldMessage.getManifoldUpperLimits().add(upperLimits); }
public static HandJointAnglePacket nextHandJointAnglePacket(Random random) { HandJointAnglePacket next = new HandJointAnglePacket(); next.setRobotSide(RandomNumbers.nextEnum(random, RobotSide.class).toByte()); next.getJointAngles().add(RandomNumbers.nextDoubleArray(random, random.nextInt(1000), 1.0)); next.setConnected(random.nextBoolean()); next.setCalibrated(random.nextBoolean()); return next; }
public static DesiredAccelerationsMessage nextDesiredAccelerationsMessage(Random random) { DesiredAccelerationsMessage next = new DesiredAccelerationsMessage(); next.getDesiredJointAccelerations().add(RandomNumbers.nextDoubleArray(random, random.nextInt(16) + 1, 1.0)); next.getQueueingProperties().set(nextQueueableMessage(random)); return next; }