diff --git a/curves/include/curves/CubicHermiteSE3Curve.hpp b/curves/include/curves/CubicHermiteSE3Curve.hpp
index 33d604d..03c00e4 100644
--- a/curves/include/curves/CubicHermiteSE3Curve.hpp
+++ b/curves/include/curves/CubicHermiteSE3Curve.hpp
@@ -275,6 +275,7 @@ typedef kindr::HomogeneousTransformationPosition3RotationQuaternionD SE3;
 typedef SE3::Rotation SO3;
 typedef kindr::AngleAxisPD AngleAxis;
 typedef kindr::RotationQuaternionPD RotationQuaternion;
+typedef kindr::RotationVectorPD RotationVector;
 typedef Eigen::Matrix<double, 6, 1> Vector6;
 typedef kindr::TwistGlobalD Twist;
 
diff --git a/curves/src/CubicHermiteSE3Curve.cpp b/curves/src/CubicHermiteSE3Curve.cpp
index 6434c63..7f3ba49 100644
--- a/curves/src/CubicHermiteSE3Curve.cpp
+++ b/curves/src/CubicHermiteSE3Curve.cpp
@@ -347,15 +347,15 @@ bool CubicHermiteSE3Curve::evaluateDerivative(DerivativeType& derivative,
       const SO3 w2_beta2_exp = RotationQuaternion().exponentialMap((beta2) * w2);
       const SO3 w3_beta3_exp = RotationQuaternion().exponentialMap((beta3) * w3);
 
-      const RotationQuaternion w1_dbeta1(0.0, dbeta1 * w1);
-      const RotationQuaternion w2_dbeta2(0.0, dbeta2 * w2);
-      const RotationQuaternion w3_dbeta3(0.0, dbeta3 * w3);
+      const RotationQuaternion w1_dbeta1(RotationVector(dbeta1 * w1));
+      const RotationQuaternion w2_dbeta2(RotationVector(dbeta2 * w2));
+      const RotationQuaternion w3_dbeta3(RotationVector(dbeta3 * w3));
 
       const Eigen::Vector4d diff =    ((T_W_A.getRotation() * w1_beta1_exp * w1_dbeta1    * w2_beta2_exp * w3_beta3_exp).vector()
                               + (T_W_A.getRotation() * w1_beta1_exp * w2_beta2_exp * w2_dbeta2    * w3_beta3_exp).vector()
                               + (T_W_A.getRotation() * w1_beta1_exp * w2_beta2_exp * w3_beta3_exp * w3_dbeta3   ).vector())*one_over_dt_sec;
 
-      const RotationQuaternion qDiff(diff);
+      const RotationQuaternion qDiff(diff.normalized());
       ValueType q;
       if(!evaluate(q, time)) {
         return false;