diff --git a/ur_kinematics/src/ur_kin.cpp b/ur_kinematics/src/ur_kin.cpp
index c1f74bd839b8327f1a0a7e804dc84864cac25474..8e4e17a85606a1f4a4a8cb4e898e3f95837e4087 100644
--- a/ur_kinematics/src/ur_kin.cpp
+++ b/ur_kinematics/src/ur_kin.cpp
@@ -46,33 +46,25 @@ namespace ur_kinematics {
 
   void forward(const double* q, double* T) {
     double s1 = sin(*q), c1 = cos(*q); q++;
-    double q234 = *q, s2 = sin(*q), c2 = cos(*q); q++;
-    double s3 = sin(*q), c3 = cos(*q); q234 += *q; q++;
-    q234 += *q; q++;
+    double q23 = *q, q234 = *q, s2 = sin(*q), c2 = cos(*q); q++;
+    double s3 = sin(*q), c3 = cos(*q); q23 += *q; q234 += *q; q++;
+    double s4 = sin(*q), c4 = cos(*q); q234 += *q; q++;
     double s5 = sin(*q), c5 = cos(*q); q++;
     double s6 = sin(*q), c6 = cos(*q); 
+    double s23 = sin(q23), c23 = cos(q23);
     double s234 = sin(q234), c234 = cos(q234);
-    *T = ((c1*c234-s1*s234)*s5)/2.0 - c5*s1 + ((c1*c234+s1*s234)*s5)/2.0; T++;
-    *T = (c6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0) - 
-          (s6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0); T++;
-    *T = (-(c6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0 - 
-          s6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0)); T++;
-    *T = ((d5*(s1*c234-c1*s234))/2.0 - (d5*(s1*c234+c1*s234))/2.0 - 
-          d4*s1 + (d6*(c1*c234-s1*s234)*s5)/2.0 + (d6*(c1*c234+s1*s234)*s5)/2.0 - 
-          a2*c1*c2 - d6*c5*s1 - a3*c1*c2*c3 + a3*c1*s2*s3); T++;
-    *T = c1*c5 + ((s1*c234+c1*s234)*s5)/2.0 + ((s1*c234-c1*s234)*s5)/2.0; T++;
-    *T = (c6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0) + 
-          s6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0)); T++;
-    *T = (c6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0) - 
-          s6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0)); T++;
-    *T = ((d5*(c1*c234-s1*s234))/2.0 - (d5*(c1*c234+s1*s234))/2.0 + d4*c1 + 
-          (d6*(s1*c234+c1*s234)*s5)/2.0 + (d6*(s1*c234-c1*s234)*s5)/2.0 + d6*c1*c5 - 
-          a2*c2*s1 - a3*c2*c3*s1 + a3*s1*s2*s3); T++;
-    *T = ((c234*c5-s234*s5)/2.0 - (c234*c5+s234*s5)/2.0); T++;
-    *T = ((s234*c6-c234*s6)/2.0 - (s234*c6+c234*s6)/2.0 - s234*c5*c6); T++;
-    *T = (s234*c5*s6 - (c234*c6+s234*s6)/2.0 - (c234*c6-s234*s6)/2.0); T++;
-    *T = (d1 + (d6*(c234*c5-s234*s5))/2.0 + a3*(s2*c3+c2*s3) + a2*s2 - 
-         (d6*(c234*c5+s234*s5))/2.0 - d5*c234); T++;
+    *T = c234*c1*s5 - c5*s1; T++;
+    *T = c6*(s1*s5 + c234*c1*c5) - s234*c1*s6; T++;
+    *T = -s6*(s1*s5 + c234*c1*c5) - s234*c1*c6; T++;
+    *T = d6*c234*c1*s5 - a3*c23*c1 - a2*c1*c2 - d6*c5*s1 - d5*s234*c1 - d4*s1; T++;
+    *T = c1*c5 + c234*s1*s5; T++;
+    *T = -c6*(c1*s5 - c234*c5*s1) - s234*s1*s6; T++;
+    *T = s6*(c1*s5 - c234*c5*s1) - s234*c6*s1; T++;
+    *T = d6*(c1*c5 + c234*s1*s5) + d4*c1 - a3*c23*s1 - a2*c2*s1 - d5*s234*s1; T++;
+    *T = -s234*s5; T++;
+    *T = -c234*s6 - s234*c5*c6; T++;
+    *T = s234*c5*s6 - c234*c6; T++;
+    *T = d1 + a3*s23 + a2*s2 - d5*(c23*c4 - s23*s4) - d6*s5*(c23*s4 + s23*c4); T++;
     *T = 0.0; T++; *T = 0.0; T++; *T = 0.0; T++; *T = 1.0;
   }