message: Interpolat desired orientations with slerp and change orientation error to quaternion
diff: @@ -158,10 +158,7 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/,
  }
  // "difference" quaternion
  Eigen::Quaterniond error_quaternion(orientation * orientation_d_.inverse());
  - // convert to axis angle
  - Eigen::AngleAxisd error_quaternion_angle_axis(error_quaternion);
  - // compute "orientation error"
  - error.tail(3) << error_quaternion_angle_axis.axis() * error_quaternion_angle_axis.angle();
  + error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
  // compute control
  // allocate variables
  @@ -197,13 +194,7 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/,
  nullspace_stiffness_ =
  filter_params_ * nullspace_stiffness_target_ + (1.0 - filter_params_) * nullspace_stiffness_;
  position_d_ = filter_params_ * position_d_target_ + (1.0 - filter_params_) * position_d_;
  - Eigen::AngleAxisd aa_orientation_d(orientation_d_);
  - Eigen::AngleAxisd aa_orientation_d_target(orientation_d_target_);
  - aa_orientation_d.axis() = filter_params_ * aa_orientation_d_target.axis() +
  - (1.0 - filter_params_) * aa_orientation_d.axis();
  - aa_orientation_d.angle() = filter_params_ * aa_orientation_d_target.angle() +
  - (1.0 - filter_params_) * aa_orientation_d.angle();
  - orientation_d_ = Eigen::Quaterniond(aa_orientation_d);
  + orientation_d_ = orientation_d_.slerp(filter_params_, orientation_d_target_);
  }
  Eigen::Matrix<double, 7, 1> CartesianImpedanceExampleController::saturateTorqueRate(
  