public static Torque getTorque(double B, double C, double D, double theta1,
			double theta2, double theta3, double w1, double w2, double w3,
			double w4, double w5, double w6) {
		double t1;// torque for motor theta1
		double t2;// torque for motor theta2
		double t3;// torque for motor theta3
		double x1 = Math.cos(theta1 - 90) * B;
		double x2 = x1 + (Math.cos((theta2 - 90) - (90 - theta1)) * C);
		double x3 = x2
				+ (Math.sin(theta3 - (90 - (theta2 - 90) - (90 - theta1))) * D);
		t3 = (((x3 - x2) / 2) * w5) + ((x3 - x2) * w6);
		t2 = (((x2 - x1) / 2) * w3) + ((x2 - x1) * w4)
				+ ((x2 - x1) + (((x3 - x2) / 2)) * w5) + ((x3 - x1) * w6);
		t1 = ((x1 / 2) * w1) + (x1 * w2) + ((x1 + ((x2 - x1) / 2)) * w3)
				+ (x2 * w2) + ((x2 + ((x3 - x2) / 2)) * w5) + (x3 * w6);
		return new Torque(t1, t2, t3);
	}