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);
}
public void setRotationAndTranslation(RigidBodyTransform transform) {
Matrix3d rotationMatrix = new Matrix3d();
transform.getRotation(rotationMatrix);
setRotation(rotationMatrix);
Vector3d translation = new Vector3d();
transform.getTranslation(translation);
setPosition(translation);
}
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));
}
