@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();
}
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;
}
/**
* Rotate the oriented bounding box of the 3D image about the specified axis with the specified
* angle.
*
* @param transform The transform and its matrix by which to rotate the image.
*/
public void rotateFrameBy(Transform3D transform) {
double rY, rX, rZ;
double sinrX, sinrY, sinrZ, cosrX, cosrY, cosrZ;
Matrix3d matrix = new Matrix3d();
transform.get(matrix);
rY = -Math.asin(matrix.m02);
if (Math.cos(rY) != 0) {
rX = -Math.atan2(matrix.m12, matrix.m22);
rZ = Math.atan2(matrix.m01, matrix.m00);
} else {
rX = -Math.atan2(matrix.m10, matrix.m11);
rZ = 0;
}
cosrX = Math.cos(rX);
sinrX = Math.sin(rX);
cosrY = Math.cos(rY);
sinrY = Math.sin(rY);
cosrZ = Math.cos(rZ);
sinrZ = Math.sin(rZ);
matrix.m00 = cosrZ * cosrY;
matrix.m01 = -sinrZ * cosrY;
matrix.m02 = sinrY;
matrix.m10 = (cosrZ * sinrY * sinrX) + (sinrZ * cosrX);
matrix.m11 = (-sinrZ * sinrY * sinrX) + (cosrZ * cosrX);
matrix.m12 = -cosrY * sinrX;
matrix.m20 = (-cosrZ * sinrY * cosrX) + (sinrZ * sinrX);
matrix.m21 = (sinrZ * sinrY * cosrX) + (cosrZ * sinrX);
matrix.m22 = cosrY * cosrX;
m_kRotate.set(matrix);
m_akAxis[0] = new Vector3f(1.0f, 0.0f, 0.0f);
m_akAxis[1] = new Vector3f(0.0f, 1.0f, 0.0f);
m_akAxis[2] = new Vector3f(0.0f, 0.0f, 1.0f);
for (int i = 0; i < 3; i++) {
m_kRotate.transform(m_akAxis[i]);
}
orthonormalize(m_akAxis);
for (int i = 0; i < 3; i++) {
setAxis(i, m_akAxis[i]);
}
}
@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);
}
protected void jointDependentSetAndGetRotation(Matrix3d Rh_i) {
tempOrientation2.set(
owner.q_qx.getDoubleValue(),
owner.q_qy.getDoubleValue(),
owner.q_qz.getDoubleValue(),
owner.q_qs.getDoubleValue());
Rh_i.set(tempOrientation2);
}
public void setRotation(Matrix3d rotation) {
double r11, r12, r13, r21, r22, r23, r31, r32, r33;
r11 = rotation.getM00();
r12 = rotation.getM01();
r13 = rotation.getM02();
r21 = rotation.getM10();
r22 = rotation.getM11();
r23 = rotation.getM12();
r31 = rotation.getM20();
r32 = rotation.getM21();
r33 = rotation.getM22();
q_qs.set(Math.sqrt((1.0 + r11 + r22 + r33) / 4.0));
q_qx.set((r32 - r23) / (4.0 * q_qs.getDoubleValue()));
q_qy.set((r13 - r31) / (4.0 * q_qs.getDoubleValue()));
q_qz.set((r21 - r12) / (4.0 * q_qs.getDoubleValue()));
}
/**
* resolve
*
* <p>マウスの動きから生成された T3D を逆運動学サーバを使って 各ジョイントの動きに直し設定する
*
* @param transform
* @return
*/
public boolean resolve(Transform3D transform) {
_setLinkStatus(robot_.getName());
Matrix3d m3d = new Matrix3d();
Vector3d v3d = new Vector3d();
transform.get(m3d, v3d);
v3d.get(tr.p);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
tr.R[3 * i + j] = m3d.getElement(i, j);
}
}
try {
if (robot_ == null || from_ == null || to_ == null) return false;
if (!integrator_.calcCharacterInverseKinematics(
robot_.getName(), from_.getName(), to_.getName(), tr)) {
System.out.println("ik failed.");
robot_.calcForwardKinematics();
return false;
}
;
} catch (Exception e) {
e.printStackTrace();
}
DblSequenceHolder v = new DblSequenceHolder();
integrator_.getCharacterAllLinkData(robot_.getName(), LinkDataType.JOINT_VALUE, v);
robot_.setJointValues(v.value);
robot_.setJointValuesWithinLimit();
robot_.updateInitialJointValues();
_setRootJoint(robot_);
robot_.calcForwardKinematics();
return true;
}
private void computeDerivativeTerm(
FrameVector desiredAngularVelocity, FrameVector currentAngularVelocity) {
desiredAngularVelocity.changeFrame(bodyFrame);
currentAngularVelocity.changeFrame(bodyFrame);
derivativeTerm.sub(desiredAngularVelocity, currentAngularVelocity);
// Limit the maximum velocity error considered for control action
double maximumVelocityError = gains.getMaximumDerivativeError();
double velocityErrorMagnitude = derivativeTerm.length();
if (velocityErrorMagnitude > maximumVelocityError) {
derivativeTerm.scale(maximumVelocityError / velocityErrorMagnitude);
}
velocityError.set(derivativeTerm);
derivativeGainMatrix.transform(derivativeTerm.getVector());
}
private void computeProportionalTerm(FrameOrientation desiredOrientation) {
desiredOrientation.changeFrame(bodyFrame);
desiredOrientation.getQuaternion(errorQuaternion);
errorAngleAxis.set(errorQuaternion);
errorAngleAxis.setAngle(AngleTools.trimAngleMinusPiToPi(errorAngleAxis.getAngle()));
// Limit the maximum position error considered for control action
double maximumError = gains.getMaximumProportionalError();
if (errorAngleAxis.getAngle() > maximumError) {
errorAngleAxis.setAngle(Math.signum(errorAngleAxis.getAngle()) * maximumError);
}
proportionalTerm.set(errorAngleAxis.getX(), errorAngleAxis.getY(), errorAngleAxis.getZ());
proportionalTerm.scale(errorAngleAxis.getAngle());
rotationErrorInBody.set(proportionalTerm);
proportionalGainMatrix.transform(proportionalTerm.getVector());
}
private void computeIntegralTerm() {
if (gains.getMaximumIntegralError() < 1e-5) {
integralTerm.setToZero(bodyFrame);
return;
}
double integratedErrorAngle = errorAngleAxis.getAngle() * dt;
double errorIntegratedX = errorAngleAxis.getX() * integratedErrorAngle;
double errorIntegratedY = errorAngleAxis.getY() * integratedErrorAngle;
double errorIntegratedZ = errorAngleAxis.getZ() * integratedErrorAngle;
rotationErrorCumulated.add(errorIntegratedX, errorIntegratedY, errorIntegratedZ);
double errorMagnitude = rotationErrorCumulated.length();
if (errorMagnitude > gains.getMaximumIntegralError()) {
rotationErrorCumulated.scale(gains.getMaximumIntegralError() / errorMagnitude);
}
rotationErrorCumulated.getFrameTuple(integralTerm);
integralGainMatrix.transform(integralTerm.getVector());
}
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 Matrix3d getViewMatrix(int index) {
Matrix3d m = new Matrix3d();
switch (index) {
case 0:
m.setIdentity(); // C4 vertex-centered
break;
case 1:
m.rotX(-0.5 * TETRAHEDRAL_ANGLE); // C3 face-centered 2.0*Math.PI/3
Matrix3d m1 = new Matrix3d();
m1.rotZ(Math.PI / 4);
m.mul(m1);
break;
case 2:
m.rotY(Math.PI / 4); // side face-centered
break;
default:
throw new IllegalArgumentException("getViewMatrix: index out of range:" + index);
}
return m;
}
@Override
public void convertToAbsolutePosition(Vector3d point) {
M.transform(point);
point.add(origin);
}
@Override
public boolean isValid() {
return M.determinant() == 1;
}
public IfcPosition3d(Vector3d origin) {
this.origin = origin;
M = new Matrix3d();
M.setIdentity();
}
