diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 8a6280a80b3245ccb1708ec70ebc7c91bbfbeeee..e48f3f1b016c8f32e29297966c5cd359db512304 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,8 +1,8 @@
# driver source files
-SET(sources uam_task_ctrl.cpp)
+SET(sources common_fc.cpp kinematics.cpp uam_task_ctrl.cpp)
# application header files
-SET(headers uam_task_ctrl.h)
+SET(headers common_obj.h common_fc.h kinematics.h uam_task_ctrl.h)
# locate the necessary dependencies
diff --git a/src/common_fc.cpp b/src/common_fc.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b478e78d85b39d109aaa0c71167e94880cbc4e0b
--- /dev/null
+++ b/src/common_fc.cpp
@@ -0,0 +1,189 @@
+#include "common_fc.h"
+
+CCommon_Fc::CCommon_Fc()
+{}
+
+CCommon_Fc::~CCommon_Fc()
+{}
+
+Eigen::MatrixXd CCommon_Fc::WeightInvJac(const Eigen::MatrixXd& J, const Eigen::MatrixXd& invjac_delta_gain, const double& invjac_alpha_min, const double& target_dist)
+{
+ // W: Weighting matrix (motion distribution) ______________________
+ double w_min = invjac_delta_gain(0,0);
+ double w_max = invjac_delta_gain(1,0);
+ double alpha_min = invjac_alpha_min;
+
+ double tmp = 2*3.14159*((target_dist-w_min)/(w_max-w_min))-3.14159;
+ double alpha = (0.5*(1+alpha_min)) + (0.5*(1-alpha_min))*tanh(tmp);
+
+ int numjoints = J.cols();
+ Eigen::MatrixXd W = Eigen::MatrixXd::Zero(numjoints,numjoints);
+
+ // Create matrices
+ for (unsigned int ii = 0; ii < numjoints; ++ii)
+ {
+ // W Quadrotor values
+ if(ii<4)
+ W(ii,ii) = 1-alpha;
+ // W Arm values
+ else
+ W(ii,ii) = alpha;
+ }
+
+ Eigen::MatrixXd Winv = CalcPinv(W);
+
+ // weighting matrix
+ Eigen::MatrixXd temp = J*Winv*J.transpose();
+ Eigen::MatrixXd J_inverse = Winv*J.transpose()*CalcPinv(temp);
+
+ return J_inverse;
+}
+
+Eigen::MatrixXd CCommon_Fc::CalcPinv(const Eigen::MatrixXd &a)
+{
+
+ // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
+ const Eigen::MatrixXd* m;
+ Eigen::MatrixXd t;
+ Eigen::MatrixXd m_pinv;
+
+ // transpose so SVD decomp can work...
+ if ( a.rows()<a.cols() ){
+ t = a.transpose();
+ m = &t;
+ } else {
+ m = &a;
+ }
+
+ // SVD
+ //JacobiSVD<Eigen::MatrixXd> svd = m->jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
+ Eigen::JacobiSVD<Eigen::MatrixXd> svd = m->jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
+ Eigen::MatrixXd vSingular = svd.singularValues();
+ // Build a diagonal matrix with the Inverted Singular values
+ // The pseudo inverted singular matrix is easy to compute :
+ // is formed by replacing every nonzero entry by its reciprocal (inversing).
+ Eigen::MatrixXd vPseudoInvertedSingular(svd.matrixV().cols(),1);
+ for (int iRow =0; iRow<vSingular.rows(); iRow++) {
+ // Todo : Put epsilon in parameter
+ if ( fabs(vSingular(iRow))<=1e-10 ) {
+ vPseudoInvertedSingular(iRow,0)=0.;
+ }
+ else {
+ vPseudoInvertedSingular(iRow,0)=1./vSingular(iRow);
+ }
+ }
+ // A little optimization here
+ Eigen::MatrixXd mAdjointU = svd.matrixU().adjoint().block(0,0,vSingular.rows(),svd.matrixU().adjoint().cols());
+ // Pseudo-Inversion : V * S * U'
+ m_pinv = (svd.matrixV() * vPseudoInvertedSingular.asDiagonal()) * mAdjointU ;
+
+ // transpose back if necessary
+ if ( a.rows()<a.cols() )
+ return m_pinv.transpose();
+ return m_pinv;
+}
+
+Eigen::Matrix4d CCommon_Fc::GetTransform(const KDL::Frame &f)
+{
+ double yaw,pitch,roll;
+ Eigen::Matrix3d R;
+ Eigen::Matrix4d T;
+
+ // f.M.GetEulerZYX(yaw,pitch,roll);
+ f.M.GetRPY(roll,pitch,yaw);
+
+ // euler convention zyx
+ R = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) \
+ * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) \
+ * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
+ T.block(0,0,3,3)=R;
+ T(0,3) = (double)f.p.x();
+ T(1,3) = (double)f.p.y();
+ T(2,3) = (double)f.p.z();
+ T.row(3) << 0, 0, 0, 1;
+
+ return T;
+}
+
+std::string CCommon_Fc::get_datetime()
+{
+ time_t rawtime;
+ struct tm * timeinfo;
+ char buffer[80];
+
+ time (&rawtime);
+ timeinfo = localtime(&rawtime);
+
+ // strftime(buffer,80,"%d%m%Y_%I%M%S",timeinfo);
+ strftime(buffer,80,"%d%m%Y_%I%M",timeinfo);
+ std::string str(buffer);
+
+ return str;
+}
+
+void CCommon_Fc::write_to_file(const std::string& folder_name, const std::string& file_name, const Eigen::MatrixXd& data, const double& ts)
+{
+ // Get home directory
+ int myuid;
+ passwd *mypasswd;
+ std::string path;
+ myuid = getuid();
+ mypasswd = getpwuid(myuid);
+ path= mypasswd->pw_dir;
+
+ // Add Folder name
+ std::stringstream folder;
+ folder << folder_name;
+ path+= folder.str();
+
+ // Add Folder with date and time
+ // path+="/";
+ // path+=this->datetime_;
+
+ // Check if path exists, and create folder if needed
+ struct stat sb;
+ if (stat(path.c_str(), &sb) != 0)
+ {
+ mkdir(path.c_str(), S_IRWXU|S_IRGRP|S_IXGRP);
+ std::string command = "mkdir -p ";
+ std::stringstream ss;
+ ss << command << path;
+ std::string com = ss.str();
+ int ret = system(com.c_str());
+ if (WEXITSTATUS(ret)!=0)
+ std::cout << "Error creating directory." << std::endl;
+ }
+
+ path+="/";
+
+ // Create file
+ std::string FileName;
+ FileName = path;
+ FileName+= file_name;
+ std::ofstream File;
+ File.open(FileName.c_str(), std::ios::out | std::ios::app | std::ios::binary);
+
+ // Write time stamp
+ File << ts;
+ File << ",";
+
+ // Write data in Matlab format
+ for (int jj = 0; jj < data.cols(); ++jj)
+ {
+ for (int ii = 0; ii < data.rows()-1; ++ii)
+ {
+ File << data(ii,jj);
+ File << ",";
+ }
+ }
+ // last row
+ for (int jj = 0; jj < data.cols()-1; ++jj)
+ {
+ File << data(data.rows()-1,jj);
+ File << ",";
+ }
+ // last term
+ File << data(data.rows()-1,data.cols()-1);
+ File << "\n";
+ File.close();
+}
diff --git a/src/common_fc.h b/src/common_fc.h
new file mode 100644
index 0000000000000000000000000000000000000000..d956315dfcdde2856a1c1e111ddf4d494e6cde63
--- /dev/null
+++ b/src/common_fc.h
@@ -0,0 +1,88 @@
+#ifndef _COMMON_FC_H
+#define _COMMON_FC_H
+
+// MISC
+#include <math.h> /* fabs */
+#include <time.h> /* time_t, struct tm, time, localtime */
+#include <string> /* string */
+#include <stdio.h> /* input, output operations */
+#include <iostream> /* input, output operations */
+#include <pwd.h> /* password */
+#include <sys/stat.h> /* file, folder, path info */
+
+// Eigen
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Eigenvalues>
+#include <eigen3/Eigen/SVD>
+
+// KDL
+#include <kdl/frames.hpp>
+#include <kdl/frames_io.hpp>
+
+typedef struct{
+ double invjac_alpha_min; // Alpha value for gain matrix pseudo inverse.
+ Eigen::MatrixXd invjac_delta_gain; // Delta values (min and max) for gain matrix pseudo inverse W.
+}misc_fc_params;
+
+class CCommon_Fc
+{
+ private:
+
+ public:
+
+ /**
+ * \brief Constructor
+ *
+ * Class constructor.
+ *
+ */
+ CCommon_Fc();
+
+ /**
+ * \brief Destructor
+ *
+ * Class destructor.
+ *
+ */
+ ~CCommon_Fc();
+
+ /**
+ * \brief Get weighted generalized Jacobian inverse
+ *
+ * Compute the Jacobian inverse weighted depending on target distance.
+ *
+ */
+ static Eigen::MatrixXd WeightInvJac(const Eigen::MatrixXd& J, const Eigen::MatrixXd& invjac_delta_gain, const double& invjac_alpha_min, const double& target_dist);
+
+ /**
+ * \brief Matrix pseudo-inverse
+ *
+ * Compute the matrix pseudo-inverse using SVD
+ */
+ static Eigen::MatrixXd CalcPinv(const Eigen::MatrixXd &a);
+
+ /**
+ * \brief KDL Frame to Homogeneous transform
+ *
+ * Compute the conversion from KDL Frame to Homogeneous transform (Eigen Matrix 4f)
+ */
+ static Eigen::Matrix4d GetTransform(const KDL::Frame &f);
+
+ /**
+ * \brief Get system date time
+ *
+ * Obtain system date time
+ */
+ static std::string get_datetime();
+
+ /**
+ * \brief Write to file
+ *
+ * Write to file some vars to plot them externally. Input data must be an Eigen MatrixX
+ *
+ */
+ static void write_to_file(const std::string& folder_name, const std::string& file_name, const Eigen::MatrixXd& data, const double& ts);
+
+};
+
+#endif
diff --git a/src/common_obj.h b/src/common_obj.h
new file mode 100644
index 0000000000000000000000000000000000000000..6a7de72e00bdf4e3c7059f89c2d2dced661a0645
--- /dev/null
+++ b/src/common_obj.h
@@ -0,0 +1,93 @@
+
+#ifndef _COMMON_OBJ_H
+#define _COMMON_OBJ_H
+
+#include <stdio.h>
+#include <iostream>
+#include <fstream>
+#include <unistd.h>
+#include <string>
+#include <sstream>
+#include <math.h>
+
+// Eigen
+#include <eigen3/Eigen/Dense>
+
+// KDL
+#include <kdl/chain.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <kdl/chainjnttojacsolver.hpp>
+
+// Boost
+#include <boost/scoped_ptr.hpp>
+#include <boost/shared_ptr.hpp>
+
+// Arm joint
+typedef struct{
+ std::string link_parent; // Link parent name.
+ std::string link_child; // Link child name.
+ double joint_min; // Joint lower limit values.
+ double joint_max; // Joint upper limit values.
+ double mass; // Joint mass.
+}CArmJoint;
+
+// Arm
+typedef struct{
+ KDL::Chain chain; // KDL chain.
+ double mass; // mass.
+ unsigned int nj; // number of joints.
+ std::vector<CArmJoint> joint_info; // joints info.
+ Eigen::MatrixXd jnt_pos; // Joint value.
+ boost::shared_ptr<KDL::ChainFkSolverPos_recursive> jnt_to_pose_solver; // chain solver.
+ boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver; // chain solver.
+ KDL::JntArray jnt_pos_kdl; // Array of arm positions.
+ KDL::Jacobian jacobian; // Jacobian.
+ std::vector<Eigen::MatrixXd> T_base_to_joint; // Homogenous Transforms from arm base to each link.
+}CArm;
+
+// Quadrotor
+typedef struct{
+ Eigen::MatrixXd pos; // position values (joints).
+}CQuad;
+
+// UAM (Unmanned Aerial Manipulator)
+typedef struct{
+ Eigen::MatrixXd vel; // Velocities (joints).
+ Eigen::MatrixXd jacob; // Jacobian.
+}CUAM;
+
+// Homogenous Transforms
+typedef struct{
+ Eigen::Matrix4d tag_in_cam; // Tag in camera frames.
+ Eigen::Matrix4d tag_in_baselink; // Tag in base_link frames.
+ Eigen::Matrix4d cam_in_tip; // Camera in arm tip frames.
+ Eigen::Matrix4d tip_in_baselink; // Arm forward kinematics (from base_link to arm link frame).
+ Eigen::Matrix4d cam_in_baselink; // Camera in base_link frames.
+ Eigen::Matrix4d armbase_in_baselink; // Camera in base_link frames.
+}CHT;
+
+// Control Law parameters
+typedef struct{
+ bool enable_sec_task; // Enable secondary task.
+ double dt; // time interval.
+ double stabil_gain; // Gain of kinematics stabilization secondary task.
+ Eigen::MatrixXd lambda_robot; // Robot proportional gains.
+ Eigen::MatrixXd ir_vel; // Inflation radius velocity.
+ Eigen::MatrixXd vs_vel; // Primary task velocity.
+ double vs_alpha_min; // Alpha value for gain matrix pseudo inverse.
+ Eigen::MatrixXd vs_delta_gain; // Delta values (min and max) for gain matrix pseudo inverse W.
+ double ir_gain; // Gain of Inflation radius task.
+ double inf_radius; // Security distance to an obstacle (Inflation radius) to prevent Collisions.
+ double cog_gain; // Gain of CoG alignment secondary task.
+ Eigen::MatrixXd cog_vel; // Secondary task velocity.
+ Eigen::MatrixXd cog_PGxy; // X and Y of the CoG r.t. Quadrotor body inertial frame.
+ Eigen::MatrixXd cog_arm; // Arm Center of Gravity r.t. quadrotor body frame.
+ double jntlim_gain; // Gain of joint limits secondary task.
+ Eigen::MatrixXd jntlim_vel; // Joint limit task velocity.
+ Eigen::MatrixXd jntlim_pos_des; // Desired arm position for secondary task (avoiding joint limits).
+ Eigen::MatrixXd jntlim_pos_error; // Arm joint errors between current and secondary task desired joint positions
+ Eigen::MatrixXd q_rollpitch; // Quadrotor roll and pitch angles.
+ Eigen::MatrixXd v_rollpitch; // Quadrotor roll and pitch angular velocities.
+}CCtrlParams;
+
+#endif
\ No newline at end of file
diff --git a/src/kinematics.cpp b/src/kinematics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4f9641a71bec98179d145682ce4a15f93c8cb490
--- /dev/null
+++ b/src/kinematics.cpp
@@ -0,0 +1,142 @@
+#include "kinematics.h"
+
+
+void CKinematics::UAMKine(const CQuad& quad, CArm& arm, CHT& HT, CUAM& uam)
+{
+ // Get arm Homogenous transform, arm tip in base_link
+ HT.tip_in_baselink = ArmFKine(arm);
+
+ // Update Transforms
+ HT.cam_in_baselink = HT.tip_in_baselink * HT.cam_in_tip;
+ HT.tag_in_baselink = HT.cam_in_baselink * HT.tag_in_cam;
+
+ // Arm Jacobian
+ Eigen::MatrixXd Ja = ArmJac(arm);
+
+ // Quadrotor Jacobian body to inertial frames
+ Eigen::MatrixXd Jb2i = QuadJac(quad.pos);
+
+ // Inertial to body frames
+ Eigen::MatrixXd Ji2b = CCommon_Fc::CalcPinv(Jb2i);
+
+ // Jacobian from quadrotor body to camera
+ Eigen::Matrix3d skew = Eigen::Matrix3d::Zero();
+ skew(0,1)=HT.cam_in_baselink(2,3);
+ skew(0,2)=-HT.cam_in_baselink(1,3);
+ skew(1,0)=-HT.cam_in_baselink(2,3);
+ skew(1,2)=HT.cam_in_baselink(0,3);
+ skew(2,0)=HT.cam_in_baselink(1,3);
+ skew(2,1)=-HT.cam_in_baselink(0,3);
+ Eigen::MatrixXd Jb2c = Eigen::MatrixXd::Zero(6,6);
+ Jb2c.block(0,0,3,3) = Eigen::Matrix3d::Identity();
+ Jb2c.block(0,3,3,3) = skew;
+ Jb2c.block(3,3,3,3) = Eigen::Matrix3d::Identity();
+
+ // Rotation of camera in body frame
+ Eigen::Matrix3d Rc_in_b = HT.cam_in_baselink.block(0,0,3,3);
+ // Rotation of body in camera frame
+ Eigen::Matrix3d Rb_in_c = Rc_in_b.transpose();
+
+ // Transform Jacobian from body to camera frame
+ Eigen::MatrixXd Rot = Eigen::MatrixXd::Zero(6,6);
+ Rot.block(0,0,3,3)=Rb_in_c;
+ Rot.block(3,3,3,3)=Rb_in_c;
+
+ // Whole robot Jacobian
+ uam.jacob = Eigen::MatrixXd::Zero(6,6+arm.nj);
+ uam.jacob.block(0,0,6,6) = Rot*Jb2c*Ji2b;
+ uam.jacob.block(0,6,6,arm.nj)=Rot*Ja;
+}
+
+Eigen::Matrix4d CKinematics::ArmFKine(CArm& arm)
+{
+ // constructs the kdl solver in non-realtime
+ arm.jnt_to_pose_solver.reset(new KDL::ChainFkSolverPos_recursive(arm.chain));
+
+ // Create joint array
+ arm.jnt_pos_kdl = KDL::JntArray(arm.nj);
+
+ // resize the joint state vector in non-realtime
+ arm.jnt_pos_kdl.resize(arm.nj);
+
+ for(unsigned int ii=0; ii < arm.nj; ii++)
+ arm.jnt_pos_kdl.data(ii) = arm.jnt_pos(ii,0);
+
+ // computes Cartesian pose in realtime From Base_link to Arm tip
+ KDL::Frame kdlTarm;
+ arm.jnt_to_pose_solver->JntToCart(arm.jnt_pos_kdl, kdlTarm);
+
+ double roll,pitch,yaw;
+ kdlTarm.M.GetEulerZYX(yaw,pitch,roll);
+
+ Eigen::Matrix4d Tarm;
+ Eigen::Matrix3d Rarm;
+ // euler convention zyx
+ Rarm = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) \
+ * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) \
+ * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
+
+ Tarm.block(0,0,3,3) = Rarm;
+ Tarm(0,3) = (double)kdlTarm.p.data[0];
+ Tarm(1,3) = (double)kdlTarm.p.data[1];
+ Tarm(2,3) = (double)kdlTarm.p.data[2];
+ Tarm.row(3) << 0, 0, 0, 1;
+
+ return Tarm;
+}
+
+Eigen::MatrixXd CKinematics::ArmJac(CArm& arm)
+{
+ // constructs the kdl solver in non-realtime
+ arm.jnt_to_jac_solver.reset(new KDL::ChainJntToJacSolver(arm.chain));
+
+ // resize the joint state vector in non-realtime
+ arm.jacobian.resize(arm.nj);
+
+ // compute Jacobian in realtime
+ arm.jnt_to_jac_solver->JntToJac(arm.jnt_pos_kdl, arm.jacobian);
+
+ Eigen::MatrixXd Ja = Eigen::MatrixXd::Zero(6,arm.nj);
+
+ for (int ii=0; ii<6; ++ii){
+ for (unsigned int jj=0; jj<arm.nj; ++jj){
+ Ja(ii,jj) = arm.jacobian.data(ii,jj);
+ }
+ }
+ return Ja;
+}
+
+Eigen::MatrixXd CKinematics::QuadJac(const Eigen::MatrixXd& pos)
+{
+ double psi,theta,phi;
+
+ phi = pos(3,0);
+ theta = pos(4,0);
+ // psi = pos(5,0);
+ psi = 0;
+
+ Eigen::MatrixXd Jq(6,6);
+ Jq(0,0) = cos(psi)*cos(theta);
+ Jq(0,1) = sin(phi)*cos(psi)*sin(theta)-cos(phi)*sin(psi);
+ Jq(0,2) = cos(phi)*cos(psi)*sin(theta)+sin(phi)*sin(psi);
+ Jq(1,0) = sin(psi)*cos(theta);
+ Jq(1,1) = sin(phi)*sin(psi)*sin(theta)+cos(phi)*cos(psi);
+
+ Jq(1,2) = cos(phi)*sin(psi)*sin(theta)-sin(phi)*cos(psi);
+ Jq(2,0) = -sin(theta);
+ Jq(2,1) = sin(phi)*cos(theta);
+ Jq(2,2) = cos(phi)*cos(theta);
+ Jq.block(0,3,3,3) = Eigen::MatrixXd::Zero(3,3);
+ Jq.block(3,0,3,3) = Eigen::MatrixXd::Zero(3,3);
+ Jq(3,3) = 1;
+ Jq(3,4) = sin(phi)*tan(theta);
+ Jq(3,5) = cos(phi)*tan(theta);
+ Jq(4,3) = 0;
+ Jq(4,4) = cos(phi);
+ Jq(4,5) = -sin(phi);
+ Jq(5,3) = 0;
+ Jq(5,4) = sin(phi)*cos(theta);
+ Jq(5,5) = cos(phi)*cos(theta);
+
+ return Jq;
+}
\ No newline at end of file
diff --git a/src/kinematics.h b/src/kinematics.h
new file mode 100644
index 0000000000000000000000000000000000000000..14d905c3dc572927a7bfadb3fab8ac1e6c91133b
--- /dev/null
+++ b/src/kinematics.h
@@ -0,0 +1,79 @@
+#ifndef _KINEMATICS_H
+#define _KINEMATICS_H
+
+// Eigen
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Eigenvalues>
+#include <eigen3/Eigen/SVD>
+
+// KDL stuff
+#include <kdl/frames.hpp>
+#include <kdl/frames_io.hpp>
+#include <kdl/chain.hpp>
+#include <kdl/chainfksolver.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <kdl/chainiksolver.hpp>
+#include <kdl/chainiksolverpos_nr.hpp>
+#include <kdl/chainiksolvervel_wdls.hpp>
+#include <kdl/chainjnttojacsolver.hpp>
+
+// Own stuff
+#include <common_obj.h>
+#include <common_fc.h>
+
+class CKinematics
+{
+ private:
+
+ public:
+
+ /**
+ * \brief Constructor
+ *
+ * Quadarm Task Priority Control Class constructor.
+ *
+ */
+ CKinematics();
+
+ /**
+ * \brief Destructor
+ *
+ * Quadarm Task Priority Control Class destructor.
+ *
+ */
+ ~CKinematics();
+
+ /**
+ * \brief Compute Arm Forward Kinematics
+ *
+ * Compute the arm Forward Kinematics
+ *
+ */
+ static Eigen::Matrix4d ArmFKine(CArm& arm);
+
+ /**
+ * \brief Compute Arm Jacobian
+ *
+ * Compute the arm Jacobian
+ *
+ */
+ static Eigen::MatrixXd ArmJac(CArm& arm);
+
+ /**
+ * \brief Compute Quadrotor Jacobian
+ *
+ * Compute the quadrotor Jacobian
+ *
+ */
+ static Eigen::MatrixXd QuadJac(const Eigen::MatrixXd& pos);
+
+ /**
+ * \brief Unmanned Aerial Manipulator Kinematics
+ *
+ * Compute the UAM Jacobian and the position of the quadrotor
+ *
+ */
+ static void UAMKine(const CQuad& quad, CArm& arm, CHT& HT, CUAM& uam);
+};
+
+#endif
diff --git a/src/uam_task_ctrl.cpp b/src/uam_task_ctrl.cpp
index 9c4ba2f54846804a3f0c3ef6fe725d3fad03dc55..c69296524933104394747efff3598ad76aac9f98 100644
--- a/src/uam_task_ctrl.cpp
+++ b/src/uam_task_ctrl.cpp
@@ -8,7 +8,7 @@ using namespace std;
CQuadarm_Task_Priority_Ctrl::CQuadarm_Task_Priority_Ctrl()
{
- this->datetime_ = get_datetime();
+ // this->datetime_ = CCommon_Fc::get_datetime();
}
CQuadarm_Task_Priority_Ctrl::~CQuadarm_Task_Priority_Ctrl()
@@ -18,10 +18,10 @@ CQuadarm_Task_Priority_Ctrl::~CQuadarm_Task_Priority_Ctrl()
// Main public function
void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const MatrixXd& quad_dist_obs,
const goal_obj& goal,
- ht& HT,
- arm& arm,
- const quadrotor& quad,
- ctrl_params& ctrl_params,
+ CHT& HT,
+ CArm& arm,
+ const CQuad& quad,
+ CCtrlParams& ctrl_params,
MatrixXd& robot_pos,
MatrixXd& robot_vel)
{
@@ -44,7 +44,7 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const MatrixXd& qua
// Quad data
this->quad_.pos = quad.pos;
- uam_kinematics();
+ CKinematics::UAMKine(this->quad_,this->arm_,this->T_,this->uam_);
uam_hierarchical_ctrl();
// Arm positions increment
@@ -62,145 +62,6 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const MatrixXd& qua
robot_vel = this->uam_.vel;
}
-// Kinematics
-void CQuadarm_Task_Priority_Ctrl::uam_kinematics(){
-
- // Get arm Homogenous transform, arm tip in base_link
- this->T_.tip_in_baselink = arm_f_kinematics();
-
- // Update Transforms
- this->T_.cam_in_baselink = this->T_.tip_in_baselink * this->T_.cam_in_tip;
- this->T_.tag_in_baselink = this->T_.cam_in_baselink * this->T_.tag_in_cam;
-
- // Arm Jacobian
- MatrixXd Ja = arm_jacobian();
-
- // Quadrotor Jacobian body to inertial frames
- MatrixXd Jb2i = quadrotor_jacobian();
-
- // Inertial to body frames
- MatrixXd Ji2b = calcPinv(Jb2i);
-
- // Jacobian from quadrotor body to camera
- Matrix3d skew = Matrix3d::Zero();
- skew(0,1)=this->T_.cam_in_baselink(2,3);
- skew(0,2)=-this->T_.cam_in_baselink(1,3);
- skew(1,0)=-this->T_.cam_in_baselink(2,3);
- skew(1,2)=this->T_.cam_in_baselink(0,3);
- skew(2,0)=this->T_.cam_in_baselink(1,3);
- skew(2,1)=-this->T_.cam_in_baselink(0,3);
- MatrixXd Jb2c = MatrixXd::Zero(6,6);
- Jb2c.block(0,0,3,3) = Matrix3d::Identity();
- Jb2c.block(0,3,3,3) = skew;
- Jb2c.block(3,3,3,3) = Matrix3d::Identity();
-
- // Rotation of camera in body frame
- Matrix3d Rc_in_b = this->T_.cam_in_baselink.block(0,0,3,3);
- // Rotation of body in camera frame
- Matrix3d Rb_in_c = Rc_in_b.transpose();
-
- // Transform Jacobian from body to camera frame
- MatrixXd Rot = MatrixXd::Zero(6,6);
- Rot.block(0,0,3,3)=Rb_in_c;
- Rot.block(3,3,3,3)=Rb_in_c;
-
- // Whole robot Jacobian
- this->uam_.jacob = MatrixXd::Zero(6,6+this->arm_.nj);
- this->uam_.jacob.block(0,0,6,6) = Rot*Jb2c*Ji2b;
- this->uam_.jacob.block(0,6,6,this->arm_.nj)=Rot*Ja;
-}
-Matrix4d CQuadarm_Task_Priority_Ctrl::arm_f_kinematics()
-{
-
- // constructs the kdl solver in non-realtime
- this->arm_.jnt_to_pose_solver.reset(new KDL::ChainFkSolverPos_recursive(this->arm_.chain));
-
- // Create joint array
- this->arm_.jnt_pos_kdl = JntArray(this->arm_.nj);
-
- // resize the joint state vector in non-realtime
- this->arm_.jnt_pos_kdl.resize(this->arm_.nj);
-
- for(unsigned int ii=0; ii < this->arm_.nj; ii++)
- this->arm_.jnt_pos_kdl.data(ii) = this->arm_.jnt_pos(ii,0);
-
- // computes Cartesian pose in realtime From Base_link to Arm tip
- Frame kdlTarm;
- this->arm_.jnt_to_pose_solver->JntToCart(this->arm_.jnt_pos_kdl, kdlTarm);
-
- double roll,pitch,yaw;
- kdlTarm.M.GetEulerZYX(yaw,pitch,roll);
-
- Matrix4d Tarm;
- Matrix3d Rarm;
- // euler convention zyx
- Rarm = AngleAxisd(yaw, Vector3d::UnitZ()) \
- * AngleAxisd(pitch, Vector3d::UnitY()) \
- * AngleAxisd(roll, Vector3d::UnitX());
-
- Tarm.block(0,0,3,3) = Rarm;
- Tarm(0,3) = (double)kdlTarm.p.data[0];
- Tarm(1,3) = (double)kdlTarm.p.data[1];
- Tarm(2,3) = (double)kdlTarm.p.data[2];
- Tarm.row(3) << 0, 0, 0, 1;
-
- return Tarm;
-}
-MatrixXd CQuadarm_Task_Priority_Ctrl::arm_jacobian()
-{
- // constructs the kdl solver in non-realtime
- this->arm_.jnt_to_jac_solver.reset(new KDL::ChainJntToJacSolver(this->arm_.chain));
-
- // resize the joint state vector in non-realtime
- this->arm_.jacobian.resize(this->arm_.nj);
-
- // compute Jacobian in realtime
- this->arm_.jnt_to_jac_solver->JntToJac(this->arm_.jnt_pos_kdl, this->arm_.jacobian);
-
- MatrixXd Ja = MatrixXd::Zero(6,this->arm_.nj);
-
- for (int ii=0; ii<6; ++ii){
- for (unsigned int jj=0; jj<this->arm_.nj; ++jj){
- Ja(ii,jj) = this->arm_.jacobian.data(ii,jj);
- }
- }
- return Ja;
-}
-MatrixXd CQuadarm_Task_Priority_Ctrl::quadrotor_jacobian()
-{
- double psi,theta,phi;
-
- phi = this->quad_.pos(3,0);
- theta = this->quad_.pos(4,0);
- // psi = this->quad_.pos(5,0);
- psi = 0;
-
- MatrixXd Jq(6,6);
- Jq(0,0) = cos(psi)*cos(theta);
- Jq(0,1) = sin(phi)*cos(psi)*sin(theta)-cos(phi)*sin(psi);
- Jq(0,2) = cos(phi)*cos(psi)*sin(theta)+sin(phi)*sin(psi);
- Jq(1,0) = sin(psi)*cos(theta);
- Jq(1,1) = sin(phi)*sin(psi)*sin(theta)+cos(phi)*cos(psi);
-
- Jq(1,2) = cos(phi)*sin(psi)*sin(theta)-sin(phi)*cos(psi);
- Jq(2,0) = -sin(theta);
- Jq(2,1) = sin(phi)*cos(theta);
- Jq(2,2) = cos(phi)*cos(theta);
- Jq.block(0,3,3,3) = MatrixXd::Zero(3,3);
- Jq.block(3,0,3,3) = MatrixXd::Zero(3,3);
- Jq(3,3) = 1;
- Jq(3,4) = sin(phi)*tan(theta);
- Jq(3,5) = cos(phi)*tan(theta);
- Jq(4,3) = 0;
- Jq(4,4) = cos(phi);
- Jq(4,5) = -sin(phi);
- Jq(5,3) = 0;
- Jq(5,4) = sin(phi)*cos(theta);
- Jq(5,5) = cos(phi)*cos(theta);
-
- return Jq;
-}
-
// Control
void CQuadarm_Task_Priority_Ctrl::uam_hierarchical_ctrl()
{
@@ -236,7 +97,7 @@ void CQuadarm_Task_Priority_Ctrl::uam_hierarchical_ctrl()
JIR_VS.block(0,0,JIR.rows(),JIR.cols()) = JIR;
JIR_VS.block(JIR.rows()-1,0,JVS.rows(),JVS.cols())=JVS;
MatrixXd NIR_VS = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
- MatrixXd JIR_VS_pseudo = calcPinv(JIR_VS);
+ MatrixXd JIR_VS_pseudo = CCommon_Fc::CalcPinv(JIR_VS);
NIR_VS = (eye-(JIR_VS_pseudo*JIR_VS));
// task Jacobian and sigma
@@ -256,7 +117,7 @@ void CQuadarm_Task_Priority_Ctrl::uam_hierarchical_ctrl()
JIR_VS_G.block(0,0,JIR_VS.rows(),JIR_VS.cols()) = JIR_VS;
JIR_VS_G.block(JIR_VS.rows()-1,0,JG.rows(),JG.cols())=JG;
MatrixXd NIR_VS_G = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
- MatrixXd JIR_VS_G_pseudo = calcPinv(JIR_VS_G);
+ MatrixXd JIR_VS_G_pseudo = CCommon_Fc::CalcPinv(JIR_VS_G);
// Third task after secondary aligning CoG
NIR_VS_G = (eye-(JIR_VS_G_pseudo*JIR_VS_G));
@@ -369,11 +230,11 @@ void CQuadarm_Task_Priority_Ctrl::task_infrad(MatrixXd& JIR,MatrixXd& JIR_pseudo
}
// }
- MatrixXd Hinv = calcPinv(H);
+ MatrixXd Hinv = CCommon_Fc::CalcPinv(H);
// weighting matrix
MatrixXd temp = JIR*Hinv*JIR.transpose();
- JIR_pseudo = Hinv*JIR.transpose()*calcPinv(temp);
+ JIR_pseudo = Hinv*JIR.transpose()*CCommon_Fc::CalcPinv(temp);
// cout << "xxxxxx Task velocities xxxxxx" << endl;
// cout << JIR_pseudo*sigmaIR << endl;
@@ -397,8 +258,9 @@ void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,Mat
JVS = Jqa1;
// task jacobian pseudo inverse
- //JVS_pseudo = calcPinv(Jqa1);
- JVS_pseudo = weight_jacvs_inverse(Jqa1);
+ //JVS_pseudo = CCommon_Fc::CalcPinv(Jqa1);
+ // JVS_pseudo = CCommon_Fc::WeightInvJac(Jqa1);
+ JVS_pseudo = CCommon_Fc::WeightInvJac(Jqa1,this->ctrl_params_.vs_delta_gain,this->ctrl_params_.vs_alpha_min,this->target_dist_);
// task velocity vector
sigmaVS = this->cam_vel_-Jqa2*this->ctrl_params_.v_rollpitch;
@@ -460,7 +322,7 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
// Joint frames
joint_pose.at(jj) = arm_segment.at(jj+1).pose(this->arm_.jnt_pos(jj,0));
// relative Homogeneous transforms (HT)
- Transf.at(jj) = GetTransform(joint_pose.at(jj));
+ Transf.at(jj) = CCommon_Fc::GetTransform(joint_pose.at(jj));
// HT r.t. base link
T_base_to_joint.at(jj+1) = T_base_to_joint.at(jj) * Transf.at(jj);
// 3rd column of rotation matrix
@@ -561,7 +423,7 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
JG.block(0,4,1,this->arm_.nj)=2*this->ctrl_params_.cog_PGxy.transpose()*Jacob_temp.block(0,0,2,this->arm_.nj);
- JG_pseudo = calcPinv(JG);
+ JG_pseudo = CCommon_Fc::CalcPinv(JG);
sigmaG = -(this->ctrl_params_.cog_PGxy.transpose()*this->ctrl_params_.cog_PGxy);
}
@@ -591,184 +453,6 @@ void CQuadarm_Task_Priority_Ctrl::task_jl(MatrixXd& JL,MatrixXd& JL_pseudo,Matri
MatrixXd Jtemp = AAL*this->ctrl_params_.jntlim_pos_error;
JL.block(0,4,1,this->arm_.nj) = -2*Jtemp.transpose();
- JL_pseudo = calcPinv(JL);
+ JL_pseudo = CCommon_Fc::CalcPinv(JL);
}
-// MISC functions
-MatrixXd CQuadarm_Task_Priority_Ctrl::weight_jacvs_inverse(const MatrixXd& J)
-{
- // W: Weighting matrix (motion distribution) ______________________
- double w_min = this->ctrl_params_.vs_delta_gain(0,0);
- double w_max = this->ctrl_params_.vs_delta_gain(1,0);
- double alpha_min = this->ctrl_params_.vs_alpha_min;
-
- double tmp = 2*3.14159*((this->target_dist_-w_min)/(w_max-w_min))-3.14159;
- double alpha = (0.5*(1+alpha_min)) + (0.5*(1-alpha_min))*tanh(tmp);
-
- MatrixXd W = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
-
- // Create matrices
- for (unsigned int ii = 0; ii < 4+this->arm_.nj; ++ii)
- {
- // W Quadrotor values
- if(ii<4)
- W(ii,ii) = 1-alpha;
- // W Arm values
- else
- W(ii,ii) = alpha;
- }
-
- MatrixXd Winv = calcPinv(W);
-
- // weighting matrix
- MatrixXd temp = J*Winv*J.transpose();
- MatrixXd J_inverse = Winv*J.transpose()*calcPinv(temp);
-
- return J_inverse;
-}
-MatrixXd CQuadarm_Task_Priority_Ctrl::calcPinv(const MatrixXd &a){
-
- // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
- const MatrixXd* m;
- MatrixXd t;
- MatrixXd m_pinv;
-
- // transpose so SVD decomp can work...
- if ( a.rows()<a.cols() ){
- t = a.transpose();
- m = &t;
- } else {
- m = &a;
- }
-
- // SVD
- //JacobiSVD<MatrixXd> svd = m->jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
- JacobiSVD<MatrixXd> svd = m->jacobiSvd(ComputeThinU | ComputeThinV);
- MatrixXd vSingular = svd.singularValues();
- // Build a diagonal matrix with the Inverted Singular values
- // The pseudo inverted singular matrix is easy to compute :
- // is formed by replacing every nonzero entry by its reciprocal (inversing).
- MatrixXd vPseudoInvertedSingular(svd.matrixV().cols(),1);
- for (int iRow =0; iRow<vSingular.rows(); iRow++) {
- // Todo : Put epsilon in parameter
- if ( fabs(vSingular(iRow))<=1e-10 ) {
- vPseudoInvertedSingular(iRow,0)=0.;
- }
- else {
- vPseudoInvertedSingular(iRow,0)=1./vSingular(iRow);
- }
- }
- // A little optimization here
- MatrixXd mAdjointU = svd.matrixU().adjoint().block(0,0,vSingular.rows(),svd.matrixU().adjoint().cols());
- // Pseudo-Inversion : V * S * U'
- m_pinv = (svd.matrixV() * vPseudoInvertedSingular.asDiagonal()) * mAdjointU ;
-
- // transpose back if necessary
- if ( a.rows()<a.cols() )
- return m_pinv.transpose();
-
- return m_pinv;
-}
-Matrix4d CQuadarm_Task_Priority_Ctrl::GetTransform(const Frame &f)
-{
- double yaw,pitch,roll;
- Matrix3d R;
- Matrix4d T;
-
- // f.M.GetEulerZYX(yaw,pitch,roll);
- f.M.GetRPY(roll,pitch,yaw);
-
- // euler convention zyx
- R = Eigen::AngleAxisd(yaw, Vector3d::UnitZ()) \
- * Eigen::AngleAxisd(pitch, Vector3d::UnitY()) \
- * Eigen::AngleAxisd(roll, Vector3d::UnitX());
- T.block(0,0,3,3)=R;
- T(0,3) = (double)f.p.x();
- T(1,3) = (double)f.p.y();
- T(2,3) = (double)f.p.z();
- T.row(3) << 0, 0, 0, 1;
-
- return T;
-}
-
-std::string CQuadarm_Task_Priority_Ctrl::get_datetime()
-{
- time_t rawtime;
- struct tm * timeinfo;
- char buffer[80];
-
- time (&rawtime);
- timeinfo = localtime(&rawtime);
-
- // strftime(buffer,80,"%d%m%Y_%I%M%S",timeinfo);
- strftime(buffer,80,"%d%m%Y_%I%M",timeinfo);
- std::string str(buffer);
-
- return str;
-}
-void CQuadarm_Task_Priority_Ctrl::write_to_file(const string& folder_name, const string& file_name, const MatrixXd& data, const double& ts)
-{
- // Get home directory
- int myuid;
- passwd *mypasswd;
- std::string path;
- myuid = getuid();
- mypasswd = getpwuid(myuid);
- path= mypasswd->pw_dir;
-
- // Add Folder name
- std::stringstream folder;
- folder << folder_name;
- path+= folder.str();
-
- // Add Folder with date and time
- // path+="/";
- // path+=this->datetime_;
-
- // Check if path exists, and create folder if needed
- struct stat sb;
- if (stat(path.c_str(), &sb) != 0)
- {
- mkdir(path.c_str(), S_IRWXU|S_IRGRP|S_IXGRP);
- std::string command = "mkdir -p ";
- std::stringstream ss;
- ss << command << path;
- std::string com = ss.str();
- int ret = system(com.c_str());
- if (WEXITSTATUS(ret)!=0)
- std::cout << "Error creating directory." << std::endl;
- }
-
- path+="/";
-
- // Create file
- std::string FileName;
- FileName = path;
- FileName+= file_name;
- std::ofstream File;
- File.open(FileName.c_str(), std::ios::out | std::ios::app | std::ios::binary);
-
- // Write time stamp
- File << ts;
- File << ",";
-
- // Write data in Matlab format
- for (int jj = 0; jj < data.cols(); ++jj)
- {
- for (int ii = 0; ii < data.rows()-1; ++ii)
- {
- File << data(ii,jj);
- File << ",";
- }
- }
- // last row
- for (int jj = 0; jj < data.cols()-1; ++jj)
- {
- File << data(data.rows()-1,jj);
- File << ",";
- }
- // last term
- File << data(data.rows()-1,data.cols()-1);
- File << "\n";
- File.close();
-}
diff --git a/src/uam_task_ctrl.h b/src/uam_task_ctrl.h
index f993d4f9c81fefd1de9f801649486aaa2a08ca27..e9aedd8c79a96cf543042accde5bf8e6c0e76f1f 100644
--- a/src/uam_task_ctrl.h
+++ b/src/uam_task_ctrl.h
@@ -7,14 +7,11 @@
#include <unistd.h>
#include <string>
#include <sstream>
-#include <pwd.h>
#include <math.h>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Eigenvalues>
#include <eigen3/Eigen/SVD>
-#include <sys/stat.h>
-
// KDL stuff
#include <kdl/frames.hpp>
#include <kdl/frames_io.hpp>
@@ -31,7 +28,10 @@
#include <boost/scoped_ptr.hpp>
#include <boost/shared_ptr.hpp>
-#include <ctime>
+// Own libraries
+#include <common_fc.h>
+#include <common_obj.h>
+#include <kinematics.h>
using namespace Eigen;
using namespace KDL;
@@ -43,74 +43,6 @@ typedef struct{
double target_dist; // Target distance
}goal_obj;
-// Homogeneous Transforms
-typedef struct{
- Matrix4d tag_in_cam; // Tag in camera frames.
- Matrix4d tag_in_baselink; // Tag in base_link frames.
- Matrix4d cam_in_tip; // Camera in arm tip frames.
- Matrix4d tip_in_baselink; // Arm forward kinematics (from base_link to arm link frame).
- Matrix4d cam_in_baselink; // Camera in base_link frames.
- Matrix4d armbase_in_baselink; // Camera in base_link frames.
-}ht;
-
-// Arm joint definition
-typedef struct{
- string link_parent; // Link parent name.
- string link_child; // Link child name.
- double joint_min; // Joint lower limit values.
- double joint_max; // Joint upper limit values.
- double mass; // Joint mass.
-}arm_joint;
-
-// Arm definition
-typedef struct{
- Chain chain; // KDL chain.
- double mass; // mass.
- unsigned int nj; // number of joints.
- vector<arm_joint> joint_info; // joints info.
- MatrixXd jnt_pos; // Joint value.
- boost::shared_ptr<ChainFkSolverPos_recursive> jnt_to_pose_solver; // chain solver.
- boost::scoped_ptr<ChainJntToJacSolver> jnt_to_jac_solver; // chain solver.
- JntArray jnt_pos_kdl; // Array of arm positions.
- Jacobian jacobian; // Jacobian.
- vector<MatrixXd> T_base_to_joint; // Homogenous Transforms from arm base to each link.
-}arm;
-
-// Quadrotor definition
-typedef struct{
- MatrixXd pos; // position values (joints).
-}quadrotor;
-
-// UAM definition
-typedef struct{
- MatrixXd vel; // Velocities (joints).
- MatrixXd jacob; // Jacobian.
-}uam;
-
-// Control Law parameters
-typedef struct{
- bool enable_sec_task; // Enable secondary task.
- double dt; // time interval.
- double stabil_gain; // Gain of kinematics stabilization secondary task.
- MatrixXd lambda_robot; // Robot proportional gains.
- MatrixXd ir_vel; // Inflation radius velocity.
- MatrixXd vs_vel; // Primary task velocity.
- double vs_alpha_min; // Alpha value for gain matrix pseudo inverse.
- MatrixXd vs_delta_gain; // Delta values (min and max) for gain matrix pseudo inverse W.
- double ir_gain; // Gain of Inflation radius task.
- double inf_radius; // Security distance to an obstacle (Inflation radius) to prevent Collisions.
- double cog_gain; // Gain of CoG alignment secondary task.
- MatrixXd cog_vel; // Secondary task velocity.
- MatrixXd cog_PGxy; // X and Y of the CoG r.t. Quadrotor body inertial frame.
- MatrixXd cog_arm; // Arm Center of Gravity r.t. quadrotor body frame.
- double jntlim_gain; // Gain of joint limits secondary task.
- MatrixXd jntlim_vel; // Joint limit task velocity.
- MatrixXd jntlim_pos_des; // Desired arm position for secondary task (avoiding joint limits).
- MatrixXd jntlim_pos_error; // Arm joint errors between current and secondary task desired joint positions
- MatrixXd q_rollpitch; // Quadrotor roll and pitch angles.
- MatrixXd v_rollpitch; // Quadrotor roll and pitch angular velocities.
-}ctrl_params;
-
typedef struct{
vector<MatrixXd> link_cog; // Vector of link CoG relative to base
MatrixXd arm_cog; // Arm CoG coordinates w.r.t. arm base link;
@@ -133,15 +65,13 @@ class CQuadarm_Task_Priority_Ctrl
{
private:
- uam uam_; // Unmanned Aerial Manipulator.
+ CUAM uam_; // Unmanned Aerial Manipulator.
+ CArm arm_; // Arm.
+ CQuad quad_; // Quadrotor.
- arm arm_; // Arm.
+ CCtrlParams ctrl_params_; // Control Law parameters.
- quadrotor quad_; // Quadrotor.
-
- ctrl_params ctrl_params_; // Control Law parameters.
-
- ht T_; // Homogeneous Transforms.
+ CHT T_; // Homogeneous Transforms.
MatrixXd cam_vel_; // Camera velocities.
@@ -149,47 +79,7 @@ class CQuadarm_Task_Priority_Ctrl
MatrixXd quad_dist_obs_; // Quadrotor distance to obstacle.
- string datetime_; // Initial date and time.
-
- /**
- * \brief Get date and time
- *
- * Get date and time as string
- *
- */
- std::string get_datetime();
-
- /**
- * \brief Compute Unmanned Aerial Manipulator Kinematics
- *
- * Compute the UAM Jacobian and the position of the quadrotor using forward kinematics
- *
- */
- void uam_kinematics();
-
- /**
- * \brief Compute Quadrotor Jacobian
- *
- * Compute the quadrotor Jacobian
- *
- */
- MatrixXd quadrotor_jacobian();
-
- /**
- * \brief Compute Arm Jacobian
- *
- * Compute the arm Jacobian
- *
- */
- MatrixXd arm_jacobian();
-
- /**
- * \brief Compute Arm Forward Kinematics
- *
- * Compute the arm Forward Kinematics
- *
- */
- Matrix4d arm_f_kinematics();
+ // string datetime_; // Initial date and time.
/**
* \brief Compute the 9DOF control law
@@ -235,27 +125,6 @@ class CQuadarm_Task_Priority_Ctrl
*/
void task_jl(MatrixXd& JL,MatrixXd& JL_pseudo,MatrixXd& sigmaL);
- /**
- * \brief Matrix pseudo-inverse
- *
- * Compute the matrix pseudo-inverse using SVD
- */
- MatrixXd calcPinv(const MatrixXd &a);
-
- /**
- * \brief Get weighted generalized Jacobian inverse
- *
- * Compute the Jacobian inverse weighted depending on target distance
- *
- */
- MatrixXd weight_jacvs_inverse(const MatrixXd& J);
-
- /**
- * \brief KDL Frame to Homogeneous transform
- *
- * Compute the conversion from KDL Frame to Homogeneous transform (Eigen Matrix 4f)
- */
- Matrix4d GetTransform(const Frame &f);
public:
@@ -291,20 +160,12 @@ class CQuadarm_Task_Priority_Ctrl
*/
void quadarm_task_priority_ctrl(const MatrixXd& quad_dist_obs,
const goal_obj& goal,
- ht& HT,
- arm& arm,
- const quadrotor& quad,
- ctrl_params& ctrl_params,
+ CHT& HT,
+ CArm& arm,
+ const CQuad& quad,
+ CCtrlParams& CtrlParams,
MatrixXd& robot_pos,
MatrixXd& robot_vel);
- /**
- * \brief Write to file
- *
- * Write to file some vars to plot them externally. Input data must be an Eigen MatrixX
- *
- */
- void write_to_file(const string& folder_name, const string& file_name, const MatrixXd& data, const double& ts);
-
};
#endif