OrientationUtils.hpp

Go to the documentation of this file.

#pragma once

#include <Eigen/Geometry>

namespace momentumopt {

  Eigen::Vector3d orientationError(const Eigen::Quaternion<double>& desired_orientation,
                                   const Eigen::Quaternion<double>& current_orientation);

  Eigen::Vector3d requiredAngularVelocity(const Eigen::Quaternion<double>& desired_orientation,
                                          const Eigen::Quaternion<double>& current_orientation,
                                          const double& time_step, bool in_body_coordinates=false);

  Eigen::Quaternion<double> integrateAngularVelocity(const Eigen::Quaternion<double>& current_orientation,
                                                     const Eigen::Vector3d& angular_velocity,
                                                     const double& time_step, bool in_body_coordinates=false);

}