public void receivedClockMessage(long totalNsecs) {
    RigidBodyTransform pelvisPoseInMocapFrame = atomicPelvisPose.get();
    IMUDefinition[] imuDefinitions = fullRobotModel.getIMUDefinitions();
    RobotConfigurationData robotConfigurationData =
        new RobotConfigurationData(
            fullRobotModel.getOneDoFJoints(), forceSensorDefinitions, null, imuDefinitions);

    for (int sensorNumber = 0; sensorNumber < imuDefinitions.length; sensorNumber++) {
      IMUPacket imuPacket = robotConfigurationData.getImuPacketForSensor(sensorNumber);
      imuPacket.set(
          RandomTools.generateRandomVector3f(random),
          RandomTools.generateRandomQuaternion4f(random),
          RandomTools.generateRandomVector3f(random));
    }

    robotConfigurationData.setRobotMotionStatus(RobotMotionStatus.STANDING);

    robotConfigurationData.setTimestamp(totalNsecs);
    if (pelvisPoseInMocapFrame != null) {
      Vector3d translation = new Vector3d();
      Quat4d orientation = new Quat4d();
      pelvisPoseInMocapFrame.getTranslation(translation);
      pelvisPoseInMocapFrame.getRotation(orientation);
      robotConfigurationData.setRootTranslation(translation);
      robotConfigurationData.setRootOrientation(orientation);
    }
    fullRobotModel.updateFrames();
    packetCommunicator.send(robotConfigurationData);
  }
コード例 #2
0
  public void setRotationAndTranslation(RigidBodyTransform transform) {
    Matrix3d rotationMatrix = new Matrix3d();
    transform.getRotation(rotationMatrix);
    setRotation(rotationMatrix);

    Vector3d translation = new Vector3d();
    transform.getTranslation(translation);
    setPosition(translation);
  }
コード例 #3
0
  public static void packRigidBodyTransformToGeometry_msgsPose(
      RigidBodyTransform pelvisTransform, Pose pose) {
    Vector3d point = new Vector3d();
    pelvisTransform.getTranslation(point);

    Quat4d rotation = new Quat4d();
    pelvisTransform.getRotation(rotation);

    packVector3dAndQuat4dToGeometry_msgsPose(point, rotation, pose);
  }
  public void drawBox(BufferedImage image, RigidBodyTransform transform, double scale) {
    DenseMatrix64F rotation = new DenseMatrix64F(3, 3);
    Vector3d translation = new Vector3d();
    transform.getRotation(rotation);
    transform.getTranslation(translation);
    Se3_F64 targetToSensor =
        new Se3_F64(
            rotation, new Vector3D_F64(translation.getX(), translation.getY(), translation.getZ()));

    Graphics2D g2 = image.createGraphics();
    g2.setStroke(new BasicStroke(4));
    g2.setColor(java.awt.Color.RED);
    VisualizeFiducial.drawCube(targetToSensor, this.intrinsicParameters, scale, 2, g2);
    g2.finalize();
  }
  private void computeOrientationStateOutputBlock() {
    estimationFrame.getTransformToDesiredFrame(tempTransform, ReferenceFrame.getWorldFrame());
    tempTransform.getRotation(rotationFromEstimationToWorld);

    tempCenterOfMassPosition.setIncludingFrame(centerOfMassPositionPort.getData());
    tempCenterOfMassPosition.changeFrame(estimationFrame);

    ReferenceFrame referenceFrame =
        referenceFrameMap.getFrameByName(
            pointPositionMeasurementInputPort.getData().getBodyFixedReferenceFrameName());
    tempFramePoint.setIncludingFrame(
        referenceFrame,
        pointPositionMeasurementInputPort.getData().getMeasurementPointInBodyFrame());

    tempFramePoint.changeFrame(estimationFrame);
    tempFramePoint.sub(tempCenterOfMassPosition);
    tempFramePoint.scale(-1.0);

    MatrixTools.toTildeForm(tempMatrix, tempFramePoint.getPoint());
    tempMatrix.mul(rotationFromEstimationToWorld, tempMatrix);
    MatrixTools.setDenseMatrixFromMatrix3d(0, 0, tempMatrix, getOutputMatrixBlock(orientationPort));
  }