@Override public String toString() { StringBuilder builder = new StringBuilder(); builder.append("["); for(int i = 0; i < size(); i++) { if(i > 0) { builder.append(", "); } builder.append(get(i)); } builder.append("]"); return builder.toString(); }
child.add(((IDLSequence.Float) seq).get(i));
child.add(((IDLSequence.Float) seq).get(i));
public void updateFullRobotModel(KinematicsToolboxOutputStatus solution) { if (jointsHashCode != solution.getJointNameHash()) throw new RuntimeException("Hashes are different."); for (int i = 0; i < oneDoFJoints.length; i++) { float q = solution.getDesiredJointAngles().get(i); OneDoFJointBasics joint = oneDoFJoints[i]; joint.setQ(q); } Vector3D translation = solution.getDesiredRootTranslation(); rootJoint.getJointPose().setPosition(translation.getX(), translation.getY(), translation.getZ()); Quaternion orientation = solution.getDesiredRootOrientation(); rootJoint.getJointPose().getOrientation().setQuaternion(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getS()); fullRobotModelToUseForConversion.updateFrames(); }
public static Point3D32[] getDecayingWorldScan(PointCloudWorldPacket pointCloudWorldPacket) { int numberOfPoints = pointCloudWorldPacket.getDecayingWorldScan().size() / 3; Point3D32[] points = new Point3D32[numberOfPoints]; for (int i = 0; i < numberOfPoints; i++) { Point3D32 point = new Point3D32(); point.setX(pointCloudWorldPacket.getDecayingWorldScan().get(3 * i)); point.setY(pointCloudWorldPacket.getDecayingWorldScan().get(3 * i + 1)); point.setZ(pointCloudWorldPacket.getDecayingWorldScan().get(3 * i + 2)); points[i] = point; } return points; }
public void set(Float other) { resetQuick(); for(int i = 0; i < other.size(); i++) { add(other.get(i)); } }
@Override public void writeElement(int i, CDR cdr) { cdr.write_type_5(get(i)); }
public static Point3D32[] unpackLocalizationPointMap(LocalizationPointMapPacket localizationPointMapPacket) { int numberOfPoints = localizationPointMapPacket.getLocalizationPointMap().size() / 3; Point3D32[] points = new Point3D32[numberOfPoints]; for (int i = 0; i < numberOfPoints; i++) { Point3D32 point = new Point3D32(); point.setX(localizationPointMapPacket.getLocalizationPointMap().get(3 * i)); point.setY(localizationPointMapPacket.getLocalizationPointMap().get(3 * i + 1)); point.setZ(localizationPointMapPacket.getLocalizationPointMap().get(3 * i + 2)); points[i] = point; } return points; }