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 updateRobotLocationBasedOnMultisensePose(RigidBodyTransform headPose) {
RigidBodyTransform pelvisPose = new RigidBodyTransform();
RigidBodyTransform transformFromHeadToPelvis =
pelvisFrame.getTransformToDesiredFrame(headFrame);
pelvisPose.multiply(headPose, transformFromHeadToPelvis);
atomicPelvisPose.set(pelvisPose);
}
public VirtualChainDataPoint(
ReferenceFrame baseFrame,
ArrayList<ReferenceFrame> referenceFrames,
FramePoint2d centerOfMassProjection)
// public VirtualChainDataPoint(ReferenceFrame baseFrame, ArrayList<ReferenceFrame>
// referenceFrames, FramePoint centerOfMassProjection)
{
centerOfMassProjection.checkReferenceFrameMatch(baseFrame);
if (!centerOfMassProjection.getReferenceFrame().isZupFrame()) {
throw new RuntimeException("centerOfMassProjection is Not a ZUp reference frame!");
}
this.centerOfMassProjection = new FramePoint2d(centerOfMassProjection);
// centerOfMassProjection.changeFrame(baseFrame);
rotationMatrices = new ArrayList<Matrix3d>();
for (int i = 0; i < referenceFrames.size(); i++) {
ReferenceFrame virtualChainFrame = referenceFrames.get(i);
RigidBodyTransform transform3D = virtualChainFrame.getTransformToDesiredFrame(baseFrame);
Matrix3d rotationMatrix = new Matrix3d();
transform3D.get(rotationMatrix);
rotationMatrices.add(rotationMatrix);
}
}
@Override
public void computeTransform(RigidBodyTransform currXform) {
update();
CameraMountInterface cameraMount = getCameraMount();
if (isMounted() && (cameraMount != null)) {
cameraMount.getTransformToCamera(currXform);
return;
}
positionOffset.set(getCamX(), getCamY(), getCamZ());
xAxis.set(getFixX(), getFixY(), getFixZ());
fixPointNode.translateTo(getFixX(), getFixY(), getFixZ());
xAxis.sub(positionOffset);
xAxis.normalize();
zAxis.set(0.0, 0.0, 1.0);
yAxis.cross(zAxis, xAxis);
zAxis.cross(xAxis, yAxis);
rotationMatrix.setColumn(0, xAxis);
rotationMatrix.setColumn(1, yAxis);
rotationMatrix.setColumn(2, zAxis);
currXform.setRotationAndZeroTranslation(rotationMatrix);
currXform.setTranslation(positionOffset);
currXform.normalizeRotationPart();
}
protected void setPinTransform3D(RigidBodyTransform t1, Vector3d u_i, double rotAng) {
t1.setIdentity();
axisAngle.set(u_i, rotAng);
t1.setRotation(axisAngle);
// t1.setRotation();
}
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);
}
private Matrix3d computeDesiredFootRotation(
double angleToDestination, RobotSide swingLegSide, ReferenceFrame supportFootFrame) {
RigidBodyTransform supportFootToWorldTransform =
supportFootFrame.getTransformToDesiredFrame(ReferenceFrame.getWorldFrame());
Matrix3d supportFootToWorldRotation = new Matrix3d();
supportFootToWorldTransform.get(supportFootToWorldRotation);
double maxTurnInAngle = 0.25;
double maxTurnOutAngle = 0.4;
double amountToYaw;
switch (blindWalkingDirection.getEnumValue()) {
case BACKWARD:
{
amountToYaw = AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI);
break;
}
case LEFT:
{
amountToYaw =
AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI / 2.0);
break;
}
case RIGHT:
{
amountToYaw =
AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, -Math.PI / 2.0);
break;
}
case FORWARD:
{
amountToYaw = angleToDestination;
break;
}
default:
{
throw new RuntimeException("Shouldn't get here!");
}
}
if (swingLegSide == RobotSide.LEFT) {
amountToYaw = MathTools.clipToMinMax(amountToYaw, -maxTurnInAngle, maxTurnOutAngle);
} else {
amountToYaw = MathTools.clipToMinMax(amountToYaw, -maxTurnOutAngle, maxTurnInAngle);
}
Matrix3d yawRotation = new Matrix3d();
yawRotation.rotZ(amountToYaw);
Matrix3d ret = new Matrix3d();
ret.mul(yawRotation, supportFootToWorldRotation);
return ret;
}
@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testCommonUsageExample() {
String namePrefix = "point";
String nameSuffix = "toTest";
YoVariableRegistry registry = new YoVariableRegistry("myRegistry");
ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
PoseReferenceFrame poseFrame = new PoseReferenceFrame("poseFrame", new FramePose(worldFrame));
FramePoint poseFramePosition = new FramePoint(worldFrame, new Point3d(0.5, 7.7, 9.2));
poseFrame.setPositionAndUpdate(poseFramePosition);
FrameOrientation poseOrientation =
new FrameOrientation(worldFrame, new AxisAngle4d(1.2, 3.9, 4.7, 2.2));
poseFrame.setOrientationAndUpdate(poseOrientation);
YoFrameSO3TrajectoryPoint yoFrameSO3TrajectoryPoint =
new YoFrameSO3TrajectoryPoint(namePrefix, nameSuffix, registry, worldFrame, poseFrame);
SimpleSO3TrajectoryPoint simpleTrajectoryPoint = new SimpleSO3TrajectoryPoint();
double time = 3.4;
TransformableQuat4d orientation = new TransformableQuat4d(new Quat4d(0.1, 0.22, 0.34, 0.56));
orientation.normalize();
Vector3d angularVelocity = new Vector3d(1.7, 8.4, 2.2);
simpleTrajectoryPoint.set(time, orientation, angularVelocity);
yoFrameSO3TrajectoryPoint.setIncludingFrame(worldFrame, simpleTrajectoryPoint);
yoFrameSO3TrajectoryPoint.changeFrame(poseFrame);
// Do some checks:
RigidBodyTransform transformToPoseFrame = worldFrame.getTransformToDesiredFrame(poseFrame);
orientation.applyTransform(transformToPoseFrame);
transformToPoseFrame.transform(angularVelocity);
namePrefix = "point";
nameSuffix = "toVerify";
YoFrameSO3TrajectoryPoint expectedYoFrameSO3TrajectoryPoint =
new YoFrameSO3TrajectoryPoint(namePrefix, nameSuffix, registry, poseFrame);
expectedYoFrameSO3TrajectoryPoint.setTime(time);
expectedYoFrameSO3TrajectoryPoint.setOrientation(orientation);
expectedYoFrameSO3TrajectoryPoint.setAngularVelocity(angularVelocity);
assertEquals(3.4, yoFrameSO3TrajectoryPoint.getTime(), 1e-7);
assertEquals(3.4, expectedYoFrameSO3TrajectoryPoint.getTime(), 1e-7);
assertTrue(expectedYoFrameSO3TrajectoryPoint.epsilonEquals(yoFrameSO3TrajectoryPoint, 1e-10));
}
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();
}
public void doMouseDraggedRight(double dx, double dy) {
// Elevate up and down
double delX0 = camX - fixX, delY0 = camY - fixY, delZ0 = camZ - fixZ;
v3d.set(delX0, delY0, delZ0);
// double offsetDistance = v3d.length();
t3d.setRotationYawAndZeroTranslation(-dx * rotate_camera_factor);
t3d.transform(v3d);
if (!isTracking || (!isTrackingX && !isTrackingY)) {
fixX = camX - v3d.getX();
fixY = camY - v3d.getY();
}
delX0 = camX - fixX;
delY0 = camY - fixY;
delZ0 = camZ - fixZ;
// v3d.set(delX0, delY0, delZ0);
rotVector.set(-1.0, 0.0, 0.0);
rotVector.cross(new Vector3d(0.0, 0.0, -1.0), v3d);
rotAxisAngle4d.set(rotVector, dy * rotate_camera_factor / 4.0);
t3d.setRotationAndZeroTranslation(rotAxisAngle4d);
t3d.transform(v3d);
if ((v3d.getX() * delX0 > 0.0) && (v3d.getY() * delY0 > 0.0)) {
if (!isTracking || (!isTrackingX && !isTrackingY)) {
fixX = camX - v3d.getX();
fixY = camY - v3d.getY();
}
if (!isTracking || !isTrackingZ) {
fixZ = camZ - v3d.getZ();
/*
* double factor = elevate_camera_factor * offsetDistance;
* //Math.abs(camZ-fixZ); if (factor < elevate_camera_factor) factor
* = elevate_camera_factor; fixZ = camZ - v3d.z + dy factor; //fixZ
* = camZ - v3d.z + dy * elevate_factor;
*/
}
}
// transformChanged(currXform);
}
// private Matrix3d tempRotationMatrix = new Matrix3d();
protected void setFloatingTransform3D(RigidBodyTransform t1) {
// position.set(q_x.val + offset.x, q_y.val + offset.y, q_z.val + offset.z);
tempPosition1.set(q_x.getDoubleValue(), q_y.getDoubleValue(), q_z.getDoubleValue());
tempOrientation1.set(
q_qx.getDoubleValue(), q_qy.getDoubleValue(), q_qz.getDoubleValue(), q_qs.getDoubleValue());
t1.set(tempOrientation1, tempPosition1);
// // An alternate way is to hardcode from
// http://www.genesis3d.com/~kdtop/Quaternions-UsingToRepresentRotation.htm (except do the
// transpose):
// double w2 = q_qs.val * q_qs.val;
// double x2 = q_qx.val * q_qx.val;
// double y2 = q_qy.val * q_qy.val;
// double z2 = q_qz.val * q_qz.val;
//
// double wx = q_qs.val * q_qx.val;
// double wy = q_qs.val * q_qy.val;
// double wz = q_qs.val * q_qz.val;
//
// double xy = q_qx.val * q_qy.val;
// double xz = q_qx.val * q_qz.val;
// double yz = q_qy.val * q_qz.val;
//
//
//// Matrix3d rotationMatrix = new Matrix3d
//
// tempRotationMatrix = new Matrix3d(
// w2+x2-y2-z2, 2.0*(xy - wz), 2.0*(xz + wy),
// 2.0*(xy+wz), w2-x2+y2-z2, 2.0*(yz-wx),
// 2.0*(xz-wy), 2.0*(yz+wx), w2-x2-y2+z2
// );
//
// t1.set(tempRotationMatrix, tempPosition1, 1.0);
}
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");
}
@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testSingleRigidBodyRotation() {
Random random = new Random(1766L);
RigidBody elevator = new RigidBody("elevator", world);
Vector3d jointAxis = RandomTools.generateRandomVector(random);
jointAxis.normalize();
RigidBodyTransform transformToParent = new RigidBodyTransform();
transformToParent.setIdentity();
RevoluteJoint joint =
ScrewTools.addRevoluteJoint("joint", elevator, transformToParent, jointAxis);
RigidBody body =
ScrewTools.addRigidBody(
"body",
joint,
RandomTools.generateRandomDiagonalMatrix3d(random),
random.nextDouble(),
new Vector3d());
joint.setQ(random.nextDouble());
joint.setQd(random.nextDouble());
Momentum momentum = computeMomentum(elevator, world);
momentum.changeFrame(world);
FrameVector linearMomentum =
new FrameVector(momentum.getExpressedInFrame(), momentum.getLinearPartCopy());
FrameVector angularMomentum =
new FrameVector(momentum.getExpressedInFrame(), momentum.getAngularPartCopy());
FrameVector linearMomentumCheck = new FrameVector(world);
Matrix3d inertia = body.getInertia().getMassMomentOfInertiaPartCopy();
Vector3d angularMomentumCheckVector = new Vector3d(jointAxis);
angularMomentumCheckVector.scale(joint.getQd());
inertia.transform(angularMomentumCheckVector);
FrameVector angularMomentumCheck =
new FrameVector(body.getInertia().getExpressedInFrame(), angularMomentumCheckVector);
angularMomentumCheck.changeFrame(world);
double epsilon = 1e-9;
JUnitTools.assertTuple3dEquals(
linearMomentumCheck.getVector(), linearMomentum.getVector(), epsilon);
JUnitTools.assertTuple3dEquals(
angularMomentumCheck.getVector(), angularMomentum.getVector(), epsilon);
assertTrue(angularMomentum.length() > epsilon);
}
public void doMouseDraggedLeft(double dx, double dy) {
// Rotate around fix point:
double delX0 = camX - fixX, delY0 = camY - fixY, delZ0 = camZ - fixZ;
v3d.set(delX0, delY0, delZ0);
// double offsetDistance = v3d.length();
t3d.setRotationYawAndZeroTranslation(-dx * rotate_factor);
t3d.transform(v3d);
if (!isDolly || (!isDollyX && !isDollyY)) {
camX = v3d.getX() + fixX;
camY = v3d.getY() + fixY;
}
delX0 = camX - fixX;
delY0 = camY - fixY;
delZ0 = camZ - fixZ;
// v3d.set(delX0, delY0, delZ0);
rotVector.cross(new Vector3d(0.0, 0.0, -1.0), v3d);
rotAxisAngle4d.set(rotVector, dy * rotate_factor / 4.0);
t3d.setRotationAndZeroTranslation(rotAxisAngle4d);
t3d.transform(v3d);
if ((v3d.getX() * delX0 > 0.0) && (v3d.getY() * delY0 > 0.0)) {
if (!isDolly || (!isDollyX && !isDollyY)) {
camX = v3d.getX() + fixX;
camY = v3d.getY() + fixY;
}
if (!isDolly || !isDollyZ) {
camZ = v3d.getZ() + fixZ;
/*
* double factor = elevate_factor * Math.abs(offsetDistance);
* //camZ-fixZ); if (factor < elevate_factor) factor =
* elevate_factor; //camZ = v3d.z + fixZ + dy * elevate_factor; camZ
* = v3d.z + fixZ + dy * factor;
*/
}
}
// transformChanged(currXform);
}
public void applyTransform(RigidBodyTransform transform, boolean requireTransformInXYPlane) {
temporaryTransformedPoint.set(this.getX(), this.getY(), 0.0);
transform.transform(temporaryTransformedPoint);
if (requireTransformInXYPlane)
checkIsTransformationInPlane(transform, temporaryTransformedPoint);
this.set(temporaryTransformedPoint.getX(), temporaryTransformedPoint.getY());
}
private void rotateInitialOrientation(
FrameOrientation orientationToPack, double angleFromInitialOrientation) {
initialOrientation.changeFrame(circleFrame);
orientationToPack.setIncludingFrame(initialOrientation);
axisRotationTransform.rotZ(angleFromInitialOrientation);
orientationToPack.applyTransform(axisRotationTransform);
}
public void rotateAroundFix(double dx, double dy) {
double distanceFromCameraToFix =
Math.sqrt(Math.pow(camX - fixX, 2) + Math.pow(camY - fixY, 2) + Math.pow(camZ - fixZ, 2));
if (distanceFromCameraToFix > 1.0) {
dx /= distanceFromCameraToFix;
dy /= distanceFromCameraToFix;
}
double delX0 = camX - fixX, delY0 = camY - fixY, delZ0 = camZ - fixZ;
v3d.set(delX0, delY0, delZ0);
t3d.setRotationYawAndZeroTranslation(-dx * rotate_factor);
t3d.transform(v3d);
if (!isDolly || (!isDollyX && !isDollyY)) {
camX = v3d.getX() + fixX;
camY = v3d.getY() + fixY;
}
delX0 = camX - fixX;
delY0 = camY - fixY;
delZ0 = camZ - fixZ;
rotVector.cross(new Vector3d(0.0, 0.0, -1.0), v3d);
rotAxisAngle4d.set(rotVector, dy * rotate_factor / 4.0);
t3d.setRotationAndZeroTranslation(rotAxisAngle4d);
t3d.transform(v3d);
if ((v3d.getX() * delX0 > 0.0) && (v3d.getY() * delY0 > 0.0)) {
if (!isDolly || (!isDollyX && !isDollyY)) {
camX = v3d.getX() + fixX;
camY = v3d.getY() + fixY;
}
if (!isDolly || !isDollyZ) {
camZ = v3d.getZ() + fixZ;
}
}
}
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));
}
public void translate(double x, double y, double z) {
RigidBodyTransform translator = new RigidBodyTransform();
translator.setTranslationAndIdentityRotation(new Vector3d(x, y, z));
transform.multiply(translator);
}
