private void writeHeaderFile(File header, PlanarRegionSegmentationMessage[] segmentationData) throws IOException { FileWriter fileWriter = new FileWriter(header); for (PlanarRegionSegmentationMessage message : segmentationData) { Point3D32 origin = message.getOrigin(); Vector3D32 normal = message.getNormal(); fileWriter.write("regionId: "); fileWriter.write(Integer.toString(message.getRegionId())); fileWriter.write(", origin: "); fileWriter.write(origin.getX() + ", " + origin.getY() + ", " + origin.getZ()); fileWriter.write(", normal: "); fileWriter.write(normal.getX() + ", " + normal.getY() + ", " + normal.getZ()); fileWriter.write("\n"); } fileWriter.close(); }
if (Math.abs(lineDirection.getZ()) < 1.0 - 1.0e-7) yaw = (float) Math.atan2(lineDirection.getY(), lineDirection.getX()); double xyLength = Math.sqrt(lineDirection.getX() * lineDirection.getX() + lineDirection.getY() * lineDirection.getY()); pitch = (float) Math.atan2(xyLength, lineDirection.getZ());
public void rotate(Matrix3DReadOnly rotationMatrix) { for (Point3D32 vertex : vertices) { double x = rotationMatrix.getM00() * vertex.getX() + rotationMatrix.getM01() * vertex.getY(); double y = rotationMatrix.getM10() * vertex.getX() + rotationMatrix.getM11() * vertex.getY(); double z = rotationMatrix.getM20() * vertex.getX() + rotationMatrix.getM21() * vertex.getY(); vertex.set(x, y, z); } for (Vector3D32 normal : normals) { double x = rotationMatrix.getM00() * normal.getX() + rotationMatrix.getM01() * normal.getY(); double y = rotationMatrix.getM10() * normal.getX() + rotationMatrix.getM11() * normal.getY(); double z = rotationMatrix.getM20() * normal.getX() + rotationMatrix.getM21() * normal.getY(); normal.set(x, y, z); } }
Point3D32 boxCenter = newMessage.getCenter(); MeshDataHolder boxMeshDataHolder = MeshDataGenerator.Cube(boxSize.getX(), boxSize.getY(), boxSize.getZ(), true); boxMeshDataHolder = MeshDataHolder.rotate(boxMeshDataHolder, boxOrientation); boxMeshDataHolder = MeshDataHolder.translate(boxMeshDataHolder, boxCenter);
@Override public void receivedPacket(RobotConfigurationData packet) { latestRobotConfigurationData = packet; FloatingJointBasics rootJoint = fullRobotModel.getRootJoint(); TFloatArrayList newJointAngles = packet.getJointAngles(); TFloatArrayList newJointVelocities = packet.getJointAngles(); TFloatArrayList newJointTorques = packet.getJointTorques(); OneDoFJointBasics[] oneDoFJoints = fullRobotModel.getOneDoFJoints(); for (int i = 0; i < newJointAngles.size(); i++) { oneDoFJoints[i].setQ(newJointAngles.get(i)); oneDoFJoints[i].setQd(newJointVelocities.get(i)); oneDoFJoints[i].setTau(newJointTorques.get(i)); } pelvisTranslationFromRobotConfigurationData.set(packet.getRootTranslation()); pelvisOrientationFromRobotConfigurationData.set(packet.getRootOrientation()); rootJoint.getJointPose().setPosition(pelvisTranslationFromRobotConfigurationData.getX(), pelvisTranslationFromRobotConfigurationData.getY(), pelvisTranslationFromRobotConfigurationData.getZ()); rootJoint.getJointPose().getOrientation().setQuaternion(pelvisOrientationFromRobotConfigurationData.getX(), pelvisOrientationFromRobotConfigurationData.getY(), pelvisOrientationFromRobotConfigurationData.getZ(), pelvisOrientationFromRobotConfigurationData.getS()); computeDriftTransform(); rootJoint.getPredecessor().updateFramesRecursively(); yoVariableServer.update(System.currentTimeMillis()); }