added files from previous commit

src/tasks/heading.cpp

+// Own
+#include "heading.h"
+#include "common_fc.h"
+#include "kinematics.h"
+
+// MISC
+#include <iostream>      /* input, output operations */
+#include <math.h>
+
+namespace UAM {
+
+CTaskHEADING::CTaskHEADING(){}
+
+CTaskHEADING::~CTaskHEADING(){}
+
+Eigen::Vector3d CTaskHEADING::RToEulerMin(const Eigen::Matrix3d& Rot)
+{
+  Eigen::Vector3d ang;
+
+  if (Rot(2,0)!=-1.0 && Rot(2,0)!=1.0)
+  {
+    double phi1, theta1, psi1;
+    double phi2, theta2, psi2;
+
+    theta1 = -asin(Rot(2,0));
+    theta2 = 3.141592653589793 - theta1;
+    psi1 = atan2(Rot(2,1)/cos(theta1),Rot(2,2)/cos(theta1)); 
+    psi2 = atan2(Rot(2,1)/cos(theta2),Rot(2,2)/cos(theta2)); 
+    phi1 = atan2(Rot(1,0)/cos(theta1),Rot(0,0)/cos(theta1)); 
+    phi2 = atan2(Rot(1,0)/cos(theta2),Rot(0,0)/cos(theta2)); 
+
+    // TODO: This statement can cause big discontinuities in the obtained angles when 
+    // the two norms are similar.
+    if (sqrt(std::pow(theta1,2)+std::pow(psi1,2)+std::pow(phi1,2)) < sqrt(std::pow(theta2,2)+std::pow(psi2,2)+std::pow(phi2,2)))
+      ang << psi1, theta1, phi1;
+    else
+      ang << psi2, theta2, phi2; // ZYX Euler convention.
+  }
+  else
+  {
+    double phi, theta, psi;
+    phi = 0.0;
+    if (Rot(2,0)==-1.0)
+    {
+      theta = 3.141592653589793/2;
+      psi = phi + atan2(Rot(0,1),Rot(0,2)); 
+    }
+    else
+    {
+      theta = -3.141592653589793/2;
+      psi = -phi + atan2(-Rot(0,1),-Rot(0,2)); 
+    }
+    ang << phi, theta, psi;
+  }
+
+  return ang;
+}
+
+void CTaskHEADING::TaskErrorJac(const UAM::CHT& HT, const UAM::CArm& arm, const Eigen::MatrixXd& eepos_des, Eigen::MatrixXd& sigmaHEADING, Eigen::MatrixXd& JHEADING_reduced, Eigen::MatrixXd& JHEADING_pseudo_reduced)
+{
+  // Initialize sizes
+  sigmaHEADING = Eigen::MatrixXd::Zero(1,1);
+  Eigen::MatrixXd JHEADING = Eigen::MatrixXd::Zero(1,6+arm.nj);
+  Eigen::MatrixXd JHEADING_pseudo = Eigen::MatrixXd::Zero(6+arm.nj,1);
+
+  // Task Error ________________________________________________
+
+  // End-Effector Position Error 
+  Eigen::Vector3d dist_target = eepos_des.block(0,0,3,1) - HT.baselink_in_w.block(0,3,3,1);
+  double des_yaw = std::atan2(dist_target(1),dist_target(0));
+  Eigen::Vector3d act_ori = RToEulerMin(HT.baselink_in_w.block(0,0,3,3)); 
+  sigmaHEADING(0) = des_yaw - act_ori(2);
+
+  // Task Jacobian _____________________________________________
+  JHEADING(0,5) = 1.0; 
+  JHEADING_pseudo = UAM::CCommon_Fc::CalcPinv(JHEADING);
+
+  // Return reduced Jacobians (without roll and pitch columns)
+  JHEADING_reduced = Eigen::MatrixXd::Zero(1,4+arm.nj);
+  JHEADING_reduced.block(0,0,1,3) = JHEADING.block(0,0,1,3);
+  JHEADING_reduced.block(0,3,1,1+arm.nj) = JHEADING.block(0,5,1,1+arm.nj);
+  JHEADING_pseudo_reduced = Eigen::MatrixXd::Zero(4+arm.nj,1);
+  JHEADING_pseudo_reduced.block(0,0,3,1) = JHEADING_pseudo.block(0,0,3,1);
+  JHEADING_pseudo_reduced.block(3,0,1+arm.nj,1) = JHEADING_pseudo.block(5,0,1+arm.nj,1);
+}
+
+} // End of UAM namespace

src/tasks/heading.h

+#ifndef _TASK_HEADING_H
+#define _TASK_HEADING_H
+
+// Own
+#include <common_obj.h>
+
+// Eigen
+#include <eigen3/Eigen/Dense>
+
+// Other
+#include <math.h>
+#include <cmath>
+
+namespace UAM {
+
+class CTaskHEADING
+{
+  private:
+
+    /**
+    * \brief Rotation matrix to euler
+    *
+    * Returns the euler angles corresponding to the rotation matrix. Specifically returns the angles with the "shortest" path.
+    * Based on http://www.staff.city.ac.uk/%7Esbbh653/publications/euler.pdf
+    *
+    */ 
+    static Eigen::Vector3d RToEulerMin(const Eigen::Matrix3d& Rot);
+    
+  public:
+
+    /**
+    * \brief Constructor
+    *
+    * Class constructor.
+    *
+    */
+    CTaskHEADING();
+
+    /**
+    * \brief Destructor
+    *
+    * Class destructor.
+    *
+    */
+    ~CTaskHEADING();
+
+    /**
+    * \brief Task Error and Jacobians
+    *
+    * Returns the task error and Jacobians. Both error and Jacobians are computed in the same functions to avoid repeated operations.
+    *
+    */ 
+    static void TaskErrorJac(const UAM::CHT& HT, const UAM::CArm& arm, const Eigen::MatrixXd& eepos_des, Eigen::MatrixXd& sigmaHEADING, Eigen::MatrixXd& JHEADING_reduced, Eigen::MatrixXd& JHEADING_pseudo_reduced);
+};
+
+} // End of UAM namespace
+
+#endif