diff --git a/src/riemanniantrsolver.cc b/src/riemanniantrsolver.cc
index 31b3fefdc8394184f37dc0fa7c65477a2718dfd8..7b09fe799be6bf8e2b00caa73e2852048bdd2bbb 100644
--- a/src/riemanniantrsolver.cc
+++ b/src/riemanniantrsolver.cc
@@ -304,17 +304,8 @@ void RiemannianTrustRegionSolver<GridType,TargetSpace>::solve()
         // ////////////////////////////////////////////////////
         
         SolutionType newIterate = x_;
-        for (int j=0; j<newIterate.size(); j++) {
-            
-            // Add translational correction
-            for (int k=0; k<3; k++)
-                newIterate[j].r[k] += corr[j][k];
-            
-            // Add rotational correction
-            Rotation<3,double> qCorr = Rotation<3,double>::exp(corr[j][3], corr[j][4], corr[j][5]);
-            newIterate[j].q = newIterate[j].q.mult(qCorr);
-            
-        }
+        for (int j=0; j<newIterate.size(); j++) 
+            newIterate[j] = TargetSpace::exp(newIterate[j], corr[j]);
         
         /** \todo Don't always recompute oldEnergy */
         double oldEnergy = assembler_->computeEnergy(x_);
diff --git a/src/rigidbodymotion.hh b/src/rigidbodymotion.hh
index 40e8ff6e483e881c2527bd1d262f79b569288b74..58e0adc81c735ecacec1f0554001fb69f7b69491 100644
--- a/src/rigidbodymotion.hh
+++ b/src/rigidbodymotion.hh
@@ -9,7 +9,25 @@ template <int dim, class ctype=double>
 struct RigidBodyMotion
 {
     /** \brief Type of an infinitesimal rigid body motion */
-    typedef Dune::FieldVector<ctype, (dim==3) ? 6 : 3> TangentVector;
+    typedef Dune::FieldVector<ctype, dim + Rotation<dim,ctype>::TangentVector::size> TangentVector;
+
+    /** \brief The exponential map from a given point $p \in SE(d)$. */
+    static RigidBodyMotion<dim,ctype> exp(const RigidBodyMotion<dim,ctype>& p, const TangentVector& v) {
+
+        RigidBodyMotion<dim,ctype> result;
+
+        // Add translational correction
+        for (int i=0; i<dim; i++)
+            result.r[i] = p.r[i] + v[i];
+
+        // Add rotational correction
+        typename Rotation<dim,ctype>::TangentVector qCorr;
+        for (int i=0; i<Rotation<dim,ctype>::TangentVector::size; i++)
+            qCorr[i] = v[dim+i];
+
+        result.q = Rotation<dim,ctype>::exp(p.q, qCorr);
+        return result;
+    }
 
     /** \brief Compute difference vector from a to b on the tangent space of a */
     static TangentVector difference(const RigidBodyMotion<dim,ctype>& a,
diff --git a/src/rotation.hh b/src/rotation.hh
index c20900fa4959dc4c0a0ec43c5fe487a6523527b7..980615545ee7190e8d0ebe1799541b0bcaa49f98 100644
--- a/src/rotation.hh
+++ b/src/rotation.hh
@@ -89,7 +89,7 @@ public:
 
     /** \brief The exponential map from \f$ \mathfrak{so}(3) \f$ to \f$ SO(3) \f$
      */
-    static Quaternion<T> exp(const Dune::FieldVector<T,3>& v) {
+    static Rotation<3,T> exp(const Dune::FieldVector<T,3>& v) {
         return exp(v[0], v[1], v[2]);
     }
 
@@ -114,6 +114,12 @@ public:
         return q;
     }
 
+    /** \brief The exponential map from a given point $p \in SO(3)$. */
+    static Rotation<3,T> exp(const Rotation<3,T>& p, const TangentVector& v) {
+        Rotation<3,T> corr = exp(v);
+        return p.mult(corr);
+    }
+
     static Dune::FieldMatrix<T,4,3> Dexp(const Dune::FieldVector<T,3>& v) {
 
         Dune::FieldMatrix<T,4,3> result(0);