Java Code Examples for javax.vecmath.Matrix4d#mul()

static Matrix4d combineTransformation(Matrix4d matrix, Vector3d translation, Vector3d rotation) {
    Matrix4d gM = new Matrix4d(matrix);
    Matrix4d m = new Matrix4d();

    m.setIdentity();
    m.setTranslation(translation);
    gM.mul(m);

    m.setIdentity();
    m.rotZ(rotation.z);
    gM.mul(m);

    m.setIdentity();
    m.rotY(rotation.y);
    gM.mul(m);

    m.setIdentity();
    m.rotX(rotation.x);
    gM.mul(m);

    return gM;
}
 

/**
 * Return the transformation that needs to be applied to a
 * repeat in order to superimpose onto repeat 0.
 *
 * @param repeat the repeat index
 * @return transformation matrix for the repeat
 */
public Matrix4d getRepeatTransform(int repeat){

	Matrix4d transform = new Matrix4d();
	transform.setIdentity();

	int[] counts = getAxisCounts(repeat);

	for(int t = counts.length-1; t>=0; t--) {
		if( counts[t] == 0 )
			continue;
		Matrix4d axis = new Matrix4d(axes.get(t).getOperator());
		for(int i=0;i<counts[t];i++) {
			transform.mul(axis);
		}
	}
	return transform;
}
 

protected void rotationInternal(Matrix4d rotation) {
	// multiply robot origin by target matrix to get target matrix in world space.
	
	// invert frame of reference to transform world target matrix into frame of reference space.
	Matrix4d ifor = new Matrix4d(FOR);
	ifor.invert();

	Matrix4d subjectRotation = new Matrix4d(startMatrix);		
	Matrix4d subjectAfterRotation = new Matrix4d(FOR);
	subjectAfterRotation.mul(rotation);
	subjectAfterRotation.mul(ifor);
	subjectAfterRotation.mul(subjectRotation);

	resultMatrix.set(subjectAfterRotation);
	resultMatrix.setTranslation(MatrixHelper.getPosition(startMatrix));
}
 

@Override
public void setPoseWorld(Matrix4d newPose) {
	Matrix4d newRelativePose;
	if(parent instanceof PoseEntity) {
		PoseEntity pe = (PoseEntity)parent;
		newRelativePose=pe.getPoseWorld();
		newRelativePose.invert();
		newRelativePose.mul(newPose);
	} else {
		newRelativePose=newPose;
	}
	
	setPose(newRelativePose);
}
 

/**
 * Recursive helper
 * @param symmAxes output list
 * @param prior transformation aligning the first repeat of this axis with the first overall
 * @param level current level
 */
private void getSymmetryAxes(List<Axis> symmAxes, Matrix4d prior, int level, int firstRepeat) {
	if(level >= getNumLevels() ) {
		return;
	}

	Axis elem = axes.get(level);
	Matrix4d elemOp = elem.getOperator();

	// Current axis:
	// elementary maps B -> A
	// prior maps I -> A and J -> B
	// want J -> I = J -> B -> A <- I= inv(prior) * elementary * prior
	Matrix4d currAxisOp = new Matrix4d(prior);
	currAxisOp.invert();
	currAxisOp.mul(elemOp);
	currAxisOp.mul(prior);
	Axis currAxis = new Axis(currAxisOp,elem.getOrder(),elem.getSymmType(),level,firstRepeat);
	symmAxes.add(currAxis);

	//Remember that all degrees are at least 2
	getSymmetryAxes(symmAxes,prior,level+1,firstRepeat);
	//New prior is elementary^d*prior
	Matrix4d newPrior = new Matrix4d(elemOp);
	newPrior.mul(prior);
	int childSize = getNumRepeats(level+1);
	getSymmetryAxes(symmAxes,newPrior,level+1,firstRepeat+childSize);
	for(int d=2;d<elem.getOrder();d++) {
		newPrior.mul(elemOp,newPrior);
		getSymmetryAxes(symmAxes,newPrior,level+1,firstRepeat+childSize*d);
	}
}
 

/**
 * This method is the main function call to extract all step parameters, pairing parameters, and sequence
 * information from the Structure object provided to the constructor.
 * @return This same object with the populated data, convenient for output
 *  (e.g. <i>log.info(new BasePairParameters(structure).analyze());</i>)
 */
public BasePairParameters analyze() {
	if (structure == null) {
		pairingParameters = null;
		stepParameters = null;
		return this;
	}
	List<Chain> nucleics = this.getNucleicChains(nonredundant);
	List<Pair<Group>> pairs = this.findPairs(nucleics);
	this.pairingParameters = new double[pairs.size()][6];
	this.stepParameters = new double[pairs.size()][6];
	Matrix4d lastStep;
	Matrix4d currentStep = null;
	for (int i = 0; i < pairs.size(); i++) {
		lastStep = currentStep;
		currentStep = this.basePairReferenceFrame(pairs.get(i));
		referenceFrames.add((Matrix4d)currentStep.clone());
		for (int j = 0; j < 6; j++) pairingParameters[i][j] = pairParameters[j];
		if (i != 0) {
			lastStep.invert();
			lastStep.mul(currentStep);
			double[] sparms = calculateTp(lastStep);
			for (int j = 0; j < 6; j++) stepParameters[i][j] = sparms[j];
		}
	}
	return this;
}
 

private ArrayList<BiologicalAssemblyTransformation> getBioUnitTransformationsListBinaryOperators(String assemblyId, List<PdbxStructAssemblyGen> psags) {

		ArrayList<BiologicalAssemblyTransformation> transformations = new ArrayList<>();

		List<OrderedPair<String>> operators = operatorResolver.getBinaryOperators();


		for ( PdbxStructAssemblyGen psag : psags){
			if ( psag.getAssembly_id().equals(assemblyId)) {

				List<String>asymIds= Arrays.asList(psag.getAsym_id_list().split(","));

				operatorResolver.parseOperatorExpressionString(psag.getOper_expression());

				// apply binary operators to the specified chains
				// Example 1M4X: generates all products of transformation matrices (1-60)(61-88)
				for (String chainId : asymIds) {

					for (OrderedPair<String> operator : operators) {
						Matrix4d original1 = allTransformations.get(operator.getElement1());
						Matrix4d original2 = allTransformations.get(operator.getElement2());
						if (original1 == null || original2 == null) {
							logger.warn("Could not find matrix operator for operator id {} or {}. Assembly id {} will not contain the composed operator.", operator.getElement1(), operator.getElement2(), assemblyId);
							continue;
						}
						Matrix4d composed = new Matrix4d(original1);
						composed.mul(original2);
						BiologicalAssemblyTransformation transform = new BiologicalAssemblyTransformation();
						transform.setChainId(chainId);
						transform.setId(operator.getElement1() + COMPOSED_OPERATOR_SEPARATOR + operator.getElement2());
						transform.setTransformationMatrix(composed);
						transformations.add(transform);
					}
				}
			}

		}

		return transformations;
	}
 

/**
 * Returns the combination (product) of two biological assembly transformations.
 * @param matrix1
 * @param matrix2
 * @return combined transformation
 */
public static BiologicalAssemblyTransformation combine(BiologicalAssemblyTransformation matrix1, BiologicalAssemblyTransformation matrix2) {
	Matrix4d transformation = new Matrix4d(matrix1.transformation);
	transformation.mul(matrix2.transformation);
	BiologicalAssemblyTransformation combined = new BiologicalAssemblyTransformation();
	combined.setTransformationMatrix(transformation);
	return combined;
}
 

/**
 * Returns true if the given CrystalTransform is equivalent to this one.
 * Two crystal transforms are equivalent if one is the inverse of the other, i.e.
 * their transformation matrices multiplication is equal to the identity.
 * @param other
 * @return
 */
public boolean isEquivalent(CrystalTransform other) {
	Matrix4d mul = new Matrix4d();
	mul.mul(this.matTransform,other.matTransform);

	if (mul.epsilonEquals(IDENTITY, 0.0001)) {
		return true;
	}
	return false;
}
 

private void a(Vector3f vector, clz var1) {
	if (var1 != null) {
		Matrix4d var2 = this.a();
		Vector3f var3 = new Vector3f(0.0F, 0.0F, 0.0F);
		switch (var1.b) {
			case a:
				var2.mul(a(new AxisAngle4d(1.0D, 0.0D, 0.0D, var1.c * 0.017453292519943295D)));
				var3.set(0.0F, 1.0F, 1.0F);
				break;
			case b:
				var2.mul(a(new AxisAngle4d(0.0D, 1.0D, 0.0D, var1.c * 0.017453292519943295D)));
				var3.set(1.0F, 0.0F, 1.0F);
				break;
			case c:
				var2.mul(a(new AxisAngle4d(0.0D, 0.0D, 1.0D, var1.c * 0.017453292519943295D)));
				var3.set(1.0F, 1.0F, 0.0F);
		}

		if (var1.d) {
			if (Math.abs(var1.c) == 22.5F) {
				var3.scale(fieldA);
			} else {
				var3.scale(fieldB);
			}

			var3.add(new Vector3f(1.0F, 1.0F, 1.0F));
		} else {
			var3.set(1.0F, 1.0F, 1.0F);
		}

		this.a(vector, new Vector3f(var1.a), var2, var3);
	}
}
 

/**
 * Starting from a known local origin and a known local hand position, calculate the travel for the given pose.
 * @param robot The DHRobot description. 
 * @param targetMatrix the pose that robot is attempting to reach in this solution.
 * @param keyframe store the computed solution in keyframe.
 */
@Override
public SolutionType solve(DHRobotEntity robot,Matrix4d targetMatrix,DHKeyframe keyframe) {
	Matrix4d targetPoseAdj = new Matrix4d(targetMatrix);
	
	if(robot.dhTool!=null) {
		// there is a transform between the wrist and the tool tip.
		// use the inverse to calculate the wrist Z axis and wrist position.
		robot.dhTool.refreshPoseMatrix();
		Matrix4d inverseToolPose = new Matrix4d(robot.dhTool.getPose());
		inverseToolPose.invert();
		targetPoseAdj.mul(inverseToolPose);
	}
	
	Matrix4d m4 = new Matrix4d(targetPoseAdj);

	keyframe.fkValues[0] = m4.m23;
	keyframe.fkValues[1] = m4.m03;
	keyframe.fkValues[2] = m4.m13;
 
	
	if(true) {
		Log.message("solution={"+StringHelper.formatDouble(keyframe.fkValues[0])+","+
				StringHelper.formatDouble(keyframe.fkValues[1])+","+
				StringHelper.formatDouble(keyframe.fkValues[2])+"}");
	}
	return SolutionType.ONE_SOLUTION;
}
 

/**
 * Starting from a known local origin and a known local hand position, calculate the angles for the given pose.
 * @param robot The DHRobot description. 
 * @param targetMatrix the pose that robot is attempting to reach in this solution.
 * @param keyframe store the computed solution in keyframe.
 */
@SuppressWarnings("unused")
@Override
public SolutionType solve(DHRobotEntity robot,Matrix4d targetMatrix,DHKeyframe keyframe) {
	DHLink link4 = robot.links.get(robot.links.size()-1);

	Matrix4d targetPoseAdj = new Matrix4d(targetMatrix);
	
	if(robot.dhTool!=null) {
		// there is a transform between the wrist and the tool tip.
		// use the inverse to calculate the wrist Z axis and wrist position.
		robot.dhTool.refreshPoseMatrix();
		Matrix4d inverseToolPose = new Matrix4d(robot.dhTool.getPose());
		inverseToolPose.invert();
		targetPoseAdj.mul(inverseToolPose);
	}
	Matrix4d m4 = new Matrix4d(targetPoseAdj);
	
	Point3d p4 = new Point3d(m4.m03,m4.m13,m4.m23);

	double a1 = robot.links.get(0).getR();
	double a2 = robot.links.get(1).getR();
	
	double b = a1;
	double a = a2;
	double c = Math.sqrt(p4.x*p4.x+p4.y*p4.y);
	if(c>a+b) c=a+b;

	// law of cosines to get the angle at the elbow
	// phi = acos( (b^2 - a^2 - c^2) / (-2*a*c) )
	double phi = Math.acos((b*b+a*a-c*c)/(2*a*b));

	keyframe.fkValues[1]=180-Math.toDegrees(phi);  // TODO explain this 180- here.
	
	// The the base rotation
	double theta2 = Math.PI - phi;
	double cc = a2 * Math.sin(theta2);
	double bb = a2 * Math.cos(theta2) + a1;
	double aa = c;
	double phi2 = Math.atan2(cc,bb);
	double tau = Math.atan2(p4.y,p4.x); 
	keyframe.fkValues[0]=Math.toDegrees(tau-phi2); 
	
	Point3d p3 = new Point3d(p4);
	p3.z = robot.links.get(0).getD();
	Point3d p2 = new Point3d(Math.cos(phi),Math.sin(phi),p3.z);
	
	// the height
	keyframe.fkValues[2]=-(p3.z-p4.z);
	
	// the rotation at the end effector
	Vector4d relativeX = new Vector4d(p3.x-p2.x,p3.y-p2.y,0,0);
	relativeX.normalize();  // normalize it
	
	Vector4d relativeY = new Vector4d(-relativeX.y,relativeX.x,0,0);

	Vector4d m4x = new Vector4d();
	m4.getColumn(1, m4x);
	
	double rX = m4x.dot(relativeX);
	double rY = m4x.dot(relativeY);
	
	keyframe.fkValues[3] = Math.toDegrees(-Math.atan2(rY,rX));
	
	if(false) {
		Log.message("solution={"+StringHelper.formatDouble(keyframe.fkValues[0])+","+
							keyframe.fkValues[1]+","+
							keyframe.fkValues[2]+","+
							StringHelper.formatDouble(keyframe.fkValues[3])+"}");
	}
	return SolutionType.ONE_SOLUTION;
}
 

/**
 * Starting from a known local origin and a known local hand position, calculate the angles for the given pose.
 * @param robot The DHRobot description. 
 * @param targetMatrix the pose that robot is attempting to reach in this solution.
 * @param keyframe store the computed solution in keyframe.
 */
@SuppressWarnings("unused")
@Override
public SolutionType solve(DHRobotEntity robot,Matrix4d targetMatrix,DHKeyframe keyframe) {
	DHLink link0 = robot.links.get(0);
	DHLink link1 = robot.links.get(1);
	DHLink link2 = robot.links.get(2);
	DHLink link3 = robot.links.get(3);
	DHLink link4 = robot.links.get(4);

	Matrix4d targetPoseAdj = new Matrix4d(targetMatrix);
	
	if(robot.dhTool!=null) {
		// there is a transform between the wrist and the tool tip.
		// use the inverse to calculate the wrist Z axis and wrist position.
		robot.dhTool.refreshPoseMatrix();
		Matrix4d inverseToolPose = new Matrix4d(robot.dhTool.getPose());
		inverseToolPose.invert();
		targetPoseAdj.mul(inverseToolPose);
	}
	
	Vector3d p4 = new Vector3d(
			targetPoseAdj.m03,
			targetPoseAdj.m13,
			targetPoseAdj.m23);

	// Work forward to get p1 position
	Point3d p1 = new Point3d(0,0,link0.getD());
	
	// (1) theta0 = atan(y07/x07);
	keyframe.fkValues[0] = Math.toDegrees(Math.atan2(p4.x,-p4.y));  // TODO explain why this isn't Math.atan2(p7.y,p7.x)
	if(false) Log.message("t0="+keyframe.fkValues[0]+"\t");
	
	// (2) C=z14
	double c = p4.z - p1.z;
	if(false) Log.message("c="+c+"\t");
	
	// (3) 
	double x15 = p4.x-p1.x;
	double y15 = p4.y-p1.y;
	double d = Math.sqrt(x15*x15 + y15*y15);
	if(false) Log.message("d="+d+"\t");
	
	// (4)
	double e = Math.sqrt(c*c + d*d);
	if(false) Log.message("e="+e+"\t");

	// (5) phi = acos( (b^2 - a^2 - e^2) / (-2*a*e) ) 
	double a = link2.getD();
	double b2 = link4.getD();
	double b1 = link3.getD();
	double b = Math.sqrt(b2*b2+b1*b1);
	if(false) Log.message("b="+b+"\t");
	
	double phi = Math.acos( (b*b-a*a-e*e) / (-2*a*e) );
	if(false) Log.message("phi="+Math.toDegrees(phi)+"\t");
	
	// (6) rho = atan2(d,c)
	double rho = Math.atan2(d,c);
	if(false) Log.message("rho="+Math.toDegrees(rho)+"\t");
	
	// (7) alpha1 = phi-rho
	keyframe.fkValues[1] = Math.toDegrees(rho - phi);
	if(false) Log.message("a1="+keyframe.fkValues[1]+"\t");
	
	// (8) omega = acos( (a^2-b^2-e^2) / (-2be) )
	double omega = Math.acos( (a*a-b*b-e*e) / (-2*b*e) );
	if(false) Log.message("omega="+Math.toDegrees(omega)+"\t");

	// (9) phi3 = phi + omega
	double phi3 = phi+omega;
	if(false) Log.message("phi3="+Math.toDegrees(phi3)+"\t");
			
	// angle of triangle j3-j2-j5 is ph4.
	// b2^2 = b^+b1^2-2*b*b1*cos(phi4)
	double phi4 = Math.acos( (b2*b2-b1*b1-b*b) / (-2*b1*b) );
	if(false) Log.message("phi4="+Math.toDegrees(phi4)+"\t");
	
	// (10) alpha2 - phi3-phi4
	keyframe.fkValues[2] = Math.toDegrees(phi3 - phi4);
	if(false) Log.message("alpha2="+keyframe.fkValues[2]+"\t");
	
	if(true) Log.message("solution={"+keyframe.fkValues[0]+","+keyframe.fkValues[1]+","+keyframe.fkValues[2]+"}");
	
	return SolutionType.ONE_SOLUTION;
}
 

/**
 * Starting from a known local origin and a known local hand position, calculate the angles for the given pose.
 * @param robot The DHRobot description. 
 * @param targetMatrix the pose that robot is attempting to reach in this solution.
 * @param keyframe store the computed solution in keyframe.
 */
@SuppressWarnings("unused")
@Override
public SolutionType solve(DHRobotEntity robot,Matrix4d targetMatrix,DHKeyframe keyframe) {
	DHLink link4 = robot.links.get(robot.links.size()-1);

	Matrix4d targetPoseAdj = new Matrix4d(targetMatrix);
	
	if(robot.dhTool!=null) {
		// there is a transform between the wrist and the tool tip.
		// use the inverse to calculate the wrist Z axis and wrist position.
		robot.dhTool.refreshPoseMatrix();
		Matrix4d inverseToolPose = new Matrix4d(robot.dhTool.getPose());
		inverseToolPose.invert();
		targetPoseAdj.mul(inverseToolPose);
	}
	
	Matrix4d m4 = new Matrix4d(targetPoseAdj);
	
	Point3d p4 = new Point3d(m4.m03,m4.m13,m4.m23);
	
	// the the base rotation
	keyframe.fkValues[0]=Math.toDegrees(Math.atan2(p4.x,-p4.y));

	// the height
	keyframe.fkValues[1]=p4.z;
	
	// the distance out from the center
	keyframe.fkValues[2] = Math.sqrt(p4.x*p4.x + p4.y*p4.y);
	
	// the rotation at the end effector
	Vector4d relativeX = new Vector4d(p4.x,p4.y,0,0);
	relativeX.scale(1/keyframe.fkValues[2]);  // normalize it
	
	Vector4d relativeY = new Vector4d(-relativeX.y,relativeX.x,0,0);

	Vector4d m4x = new Vector4d();
	m4.getColumn(1, m4x);
	
	double rX = m4x.dot(relativeX);
	double rY = m4x.dot(relativeY);
	
	keyframe.fkValues[3] = Math.toDegrees(-Math.atan2(rY,rX));
	
	if(false) {
		Log.message("solution={"+StringHelper.formatDouble(keyframe.fkValues[0])+","+
							keyframe.fkValues[1]+","+
							keyframe.fkValues[2]+","+
							StringHelper.formatDouble(keyframe.fkValues[3])+"}");
	}
	return SolutionType.ONE_SOLUTION;
}
 

/**
 * Use Forward Kinematics to approximate the Jacobian matrix for Sixi.
 * See also https://robotacademy.net.au/masterclass/velocity-kinematics-in-3d/?lesson=346
 */
public double [][] approximateJacobian(DHKeyframe keyframe) {
	double [][] jacobian = new double[6][6];
	
	double ANGLE_STEP_SIZE_DEGREES=0.5;  // degrees
	
	DHKeyframe oldPoseFK = getIKSolver().createDHKeyframe();
	getPoseFK(oldPoseFK);
	
	setPoseFK(keyframe);
	Matrix4d T = endEffector.getPoseWorld();
	
	DHKeyframe newPoseFK = getIKSolver().createDHKeyframe();
	int i=0;
	// for all adjustable joints
	for( DHLink link : links ) {
		if(link.flags == LinkAdjust.NONE) continue;
		
		// use anglesB to get the hand matrix after a tiny adjustment on one joint.
		newPoseFK.set(keyframe);
		newPoseFK.fkValues[i]+=ANGLE_STEP_SIZE_DEGREES;
		setPoseFK(newPoseFK);
		// Tnew will be different from T because of the changes in setPoseFK().
		Matrix4d Tnew = endEffector.getPoseWorld();
		
		// use the finite difference in the two matrixes
		// aka the approximate the rate of change (aka the integral, aka the velocity)
		// in one column of the jacobian matrix at this position.
		Matrix4d dT = new Matrix4d();
		dT.sub(Tnew,T);
		dT.mul(1.0/Math.toRadians(ANGLE_STEP_SIZE_DEGREES));
		
		jacobian[i][0]=dT.m03;
		jacobian[i][1]=dT.m13;
		jacobian[i][2]=dT.m23;

		// find the rotation part
		// these initialT and initialTd were found in the comments on
		// https://robotacademy.net.au/masterclass/velocity-kinematics-in-3d/?lesson=346
		// and used to confirm that our skew-symmetric matrix match theirs.
		/*
		double[] initialT = {
				 0,  0   , 1   ,  0.5963,
				 0,  1   , 0   , -0.1501,
				-1,  0   , 0   , -0.01435,
				 0,  0   , 0   ,  1 };
		double[] initialTd = {
				 0, -0.01, 1   ,  0.5978,
				 0,  1   , 0.01, -0.1441,
				-1,  0   , 0   , -0.01435,
				 0,  0   , 0   ,  1 };
		T.set(initialT);
		Td.set(initialTd);
		dT.sub(Td,T);
		dT.mul(1.0/Math.toRadians(ANGLE_STEP_SIZE_DEGREES));//*/
		
		//Log.message("T="+T);
		//Log.message("Td="+Td);
		//Log.message("dT="+dT);
		Matrix3d T3 = new Matrix3d(
				T.m00,T.m01,T.m02,
				T.m10,T.m11,T.m12,
				T.m20,T.m21,T.m22);
		//Log.message("R="+R);
		Matrix3d dT3 = new Matrix3d(
				dT.m00,dT.m01,dT.m02,
				dT.m10,dT.m11,dT.m12,
				dT.m20,dT.m21,dT.m22);
		//Log.message("dT3="+dT3);
		Matrix3d skewSymmetric = new Matrix3d();
		
		T3.transpose();  // inverse of a rotation matrix is its transpose
		skewSymmetric.mul(dT3,T3);
		
		//Log.message("SS="+skewSymmetric);
		//[  0 -Wz  Wy]
		//[ Wz   0 -Wx]
		//[-Wy  Wx   0]
		
		jacobian[i][3]=skewSymmetric.m12;
		jacobian[i][4]=skewSymmetric.m20;
		jacobian[i][5]=skewSymmetric.m01;
		
		++i;
	}
	
	// undo our changes.
	setPoseFK(oldPoseFK);
	
	return jacobian;
}
 

public void setupModels(DHRobotEntity robot) {
	material = new MaterialEntity();
	float r=0.75f;
	float g=0.15f;
	float b=0.15f;
	material.setDiffuseColor(r,g,b,1);
	
	try {
		robot.links.get(0).setModelFilename("/Sixi/anchor.stl");
		robot.links.get(1).setModelFilename("/Sixi/shoulder.stl");
		robot.links.get(2).setModelFilename("/Sixi/bicep.stl");
		robot.links.get(3).setModelFilename("/Sixi/elbow.stl");
		robot.links.get(5).setModelFilename("/Sixi/forearm.stl");
		robot.links.get(6).setModelFilename("/Sixi/wrist.stl");
		robot.links.get(7).setModelFilename("/Sixi/hand.stl");

		for( DHLink link : robot.links ) link.setModelScale(0.1f);
		robot.links.get(1).getModel().adjustOrigin(new Vector3d(0, 0, -25));
		robot.links.get(2).getModel().adjustOrigin(new Vector3d(0, -5, -25));
		robot.links.get(2).getModel().adjustRotation(new Vector3d(-11.3,0,0));
		
		robot.links.get(5).getModel().adjustOrigin(new Vector3d(0, 0, -60));
		robot.links.get(6).getModel().adjustOrigin(new Vector3d(0, 0, -70));
		robot.links.get(7).getModel().adjustOrigin(new Vector3d(0, 0, -74));

		Matrix4d rot = new Matrix4d();
		Matrix4d rotX = new Matrix4d();
		Matrix4d rotY = new Matrix4d();
		Matrix4d rotZ = new Matrix4d();
		rot.setIdentity();
		rotX.rotX((float)Math.toRadians(90));
		rotY.rotY((float)Math.toRadians(0));
		rotZ.rotZ((float)Math.toRadians(0));
		rot.set(rotX);
		rot.mul(rotY);
		rot.mul(rotZ);
		Matrix4d pose = new Matrix4d(rot);
		Vector3d adjustPos = new Vector3d(0, 5, -50);
		pose.transform(adjustPos);
		
		robot.links.get(3).getModel().adjustOrigin(adjustPos);
		robot.links.get(3).getModel().adjustRotation(new Vector3d(90, 0, 0));
	} catch (Exception e) {
		// TODO Auto-generated catch block
		e.printStackTrace();
	}
}
 

static Matrix4d combineTransformation(Matrix4d matrix, Matrix4d transformation) {
    Matrix4d gM = new Matrix4d(matrix);
    gM.mul(transformation);

    return gM;
}
 

/**
 * Checks whether given interface is NCS-redundant, i.e., an identical interface between NCS copies of
 * these molecules has already been seen, and returns this (reference) interface.
 *
 * @param interf
 *          StructureInterface
 * @return  already seen interface that is NCS-equivalent to interf,
 *          null if such interface is not found.
 */
private StructureInterface findNcsRef(StructureInterface interf) {
	if (!this.hasNcsOps()) {
		return null;
	}
	String chainIName = interf.getMoleculeIds().getFirst();
	String iOrigName = chainOrigNames.get(chainIName);

	String chainJName = interf.getMoleculeIds().getSecond();
	String jOrigName = chainOrigNames.get(chainJName);

	Matrix4d mJCryst;
	if(this.searchBeyondAU) {
		mJCryst = interf.getTransforms().getSecond().getMatTransform();
		mJCryst = crystallographicInfo.getCrystalCell().transfToOrthonormal(mJCryst);
	} else {
		mJCryst = IDENTITY;
	}

	// Let X1,...Xn be the original coords, before NCS transforms (M1...Mk)
	// current chain i: M_i * X_i
	// current chain j: Cn * M_j * X_j

	// transformation to bring chain j near X_i: M_i^(-1) * Cn * M_j
	// transformation to bring chain i near X_j: (Cn * M_j)^(-1) * M_i = (M_i^(-1) * Cn * M_j)^(-1)

	Matrix4d mChainIInv = new Matrix4d(chainNcsOps.get(chainIName));
	mChainIInv.invert();

	Matrix4d mJNcs = new Matrix4d(chainNcsOps.get(chainJName));

	Matrix4d j2iNcsOrigin = new Matrix4d(mChainIInv);
	j2iNcsOrigin.mul(mJCryst);
	//overall transformation to bring current chainj from its NCS origin to i's
	j2iNcsOrigin.mul(mJNcs);

	//overall transformation to bring current chaini from its NCS origin to j's
	Matrix4d i2jNcsOrigin = new Matrix4d(j2iNcsOrigin);
	i2jNcsOrigin.invert();

	String matchChainIdsIJ = iOrigName + jOrigName;
	String matchChainIdsJI = jOrigName + iOrigName;

	// same original chain names
	Optional<Matrix4d> matchDirect =
			visitedNcsChainPairs.computeIfAbsent(matchChainIdsIJ, k-> new HashMap<>()).entrySet().stream().
				map(r->r.getKey()).
				filter(r->r.epsilonEquals(j2iNcsOrigin,0.01)).
				findFirst();

	Matrix4d matchMatrix = matchDirect.orElse(null);
	String matchChainIds = matchChainIdsIJ;

	if(matchMatrix == null) {
		// reversed original chain names with inverted transform
		Optional<Matrix4d> matchInverse =
				visitedNcsChainPairs.computeIfAbsent(matchChainIdsJI, k-> new HashMap<>()).entrySet().stream().
				map(r->r.getKey()).
				filter(r->r.epsilonEquals(i2jNcsOrigin,0.01)).
				findFirst();
		matchMatrix = matchInverse.orElse(null);
		matchChainIds = matchChainIdsJI;
	}

	StructureInterface matchInterface = null;

	if (matchMatrix == null) {
		visitedNcsChainPairs.get(matchChainIdsIJ).put(j2iNcsOrigin,interf);
	} else {
		matchInterface = visitedNcsChainPairs.get(matchChainIds).get(matchMatrix);
	}

	return matchInterface;
}
 

/**
 * Adds translational component to rotation matrix
 * @param rotTrans
 * @param rotation
 * @return
 */
private void combineWithTranslation(Matrix4d rotation) {
	rotation.setTranslation(centroid);
	rotation.mul(rotation, centroidInverse);
}
 

/**
 * Adds translational component to rotation matrix
 * @param rotation
 * @return
 */
private void combineWithTranslation(Matrix4d rotation) {
	rotation.setTranslation(centroid);
	rotation.mul(rotation, centroidInverse);
}