private void checkIsTransformationInPlane(
RigidBodyTransform transformToNewFrame, Point3d transformedPoint) {
transformToNewFrame.getTranslation(temporaryTranslation);
if (Math.abs(temporaryTranslation.getZ() - transformedPoint.getZ()) > epsilon)
throw new RuntimeException(
"Cannot transform FramePoint2d to a plane with a different surface normal");
}
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();
}