diff --git a/src/common_fc.cpp b/src/common_fc.cpp
index b478e78d85b39d109aaa0c71167e94880cbc4e0b..4f8fb48365ac80238c0580ca9854601785062676 100644
--- a/src/common_fc.cpp
+++ b/src/common_fc.cpp
@@ -1,5 +1,7 @@
#include "common_fc.h"
+namespace UAM {
+
CCommon_Fc::CCommon_Fc()
{}
@@ -187,3 +189,5 @@ void CCommon_Fc::write_to_file(const std::string& folder_name, const std::string
File << "\n";
File.close();
}
+
+} // End of UAM namespace
diff --git a/src/common_fc.h b/src/common_fc.h
index d956315dfcdde2856a1c1e111ddf4d494e6cde63..9ba9eab0a4691832ecc09c8f93f4f099d5606eed 100644
--- a/src/common_fc.h
+++ b/src/common_fc.h
@@ -19,10 +19,7 @@
#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;
+namespace UAM{
class CCommon_Fc
{
@@ -85,4 +82,6 @@ class CCommon_Fc
};
+} // End of UAM namespace
+
#endif
diff --git a/src/common_obj.h b/src/common_obj.h
index 6a7de72e00bdf4e3c7059f89c2d2dced661a0645..b0223adf5b4ccaf17d61b2995798465ba3e3e852 100644
--- a/src/common_obj.h
+++ b/src/common_obj.h
@@ -22,72 +22,97 @@
#include <boost/scoped_ptr.hpp>
#include <boost/shared_ptr.hpp>
+namespace UAM {
+
+// Arm CoG data
+typedef struct{
+ std::vector<Eigen::MatrixXd> link_cog; // Vector of link CoG relative to base
+ Eigen::MatrixXd arm_cog; // Arm CoG coordinates w.r.t. arm base link;
+}ArmCogData;
+
// 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.
+ 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.
+ 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).
+ Eigen::MatrixXd pos; // position values (joints).
}CQuad;
// UAM (Unmanned Aerial Manipulator)
typedef struct{
- Eigen::MatrixXd vel; // Velocities (joints).
- Eigen::MatrixXd jacob; // Jacobian.
+ 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.
+ 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.
+ 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;
+// Task Eigen Values
+typedef struct{
+ Eigen::VectorXd vs; // Visual Servo Task Eigen Values
+ Eigen::VectorXd cog; // CoG Task Eigen Values
+ Eigen::VectorXd jl; // Joint Limits Task Eigen Values
+}TaskEigVal;
+
+// Task Jacobians
+typedef struct{
+ Eigen::MatrixXd ir; // Inflation Radius Task Jac
+ Eigen::MatrixXd vs; // Visual Servo Task Jac
+ Eigen::MatrixXd cog; // CoG Task Jac
+ Eigen::MatrixXd jl; // Joint Limits Task Jac
+}TaskJac;
+
+} // End of UAM namespace
+
#endif
\ No newline at end of file
diff --git a/src/kinematics.cpp b/src/kinematics.cpp
index 4f9641a71bec98179d145682ce4a15f93c8cb490..673408400811ce25ffa3e7c136b722621ed25015 100644
--- a/src/kinematics.cpp
+++ b/src/kinematics.cpp
@@ -1,7 +1,8 @@
#include "kinematics.h"
+namespace UAM {
-void CKinematics::UAMKine(const CQuad& quad, CArm& arm, CHT& HT, CUAM& uam)
+void CKinematics::UAMKine(const UAM::CQuad& quad, UAM::CArm& arm, UAM::CHT& HT, UAM::CUAM& uam)
{
// Get arm Homogenous transform, arm tip in base_link
HT.tip_in_baselink = ArmFKine(arm);
@@ -48,7 +49,7 @@ void CKinematics::UAMKine(const CQuad& quad, CArm& arm, CHT& HT, CUAM& uam)
uam.jacob.block(0,6,6,arm.nj)=Rot*Ja;
}
-Eigen::Matrix4d CKinematics::ArmFKine(CArm& arm)
+Eigen::Matrix4d CKinematics::ArmFKine(UAM::CArm& arm)
{
// constructs the kdl solver in non-realtime
arm.jnt_to_pose_solver.reset(new KDL::ChainFkSolverPos_recursive(arm.chain));
@@ -85,7 +86,7 @@ Eigen::Matrix4d CKinematics::ArmFKine(CArm& arm)
return Tarm;
}
-Eigen::MatrixXd CKinematics::ArmJac(CArm& arm)
+Eigen::MatrixXd CKinematics::ArmJac(UAM::CArm& arm)
{
// constructs the kdl solver in non-realtime
arm.jnt_to_jac_solver.reset(new KDL::ChainJntToJacSolver(arm.chain));
@@ -139,4 +140,6 @@ Eigen::MatrixXd CKinematics::QuadJac(const Eigen::MatrixXd& pos)
Jq(5,5) = cos(phi)*cos(theta);
return Jq;
-}
\ No newline at end of file
+}
+
+} // End of UAM namespace
\ No newline at end of file
diff --git a/src/kinematics.h b/src/kinematics.h
index 14d905c3dc572927a7bfadb3fab8ac1e6c91133b..514fe3a73479e61d1c922a6bc7a2db73f6f40eb8 100644
--- a/src/kinematics.h
+++ b/src/kinematics.h
@@ -21,6 +21,8 @@
#include <common_obj.h>
#include <common_fc.h>
+namespace UAM {
+
class CKinematics
{
private:
@@ -49,7 +51,7 @@ class CKinematics
* Compute the arm Forward Kinematics
*
*/
- static Eigen::Matrix4d ArmFKine(CArm& arm);
+ static Eigen::Matrix4d ArmFKine(UAM::CArm& arm);
/**
* \brief Compute Arm Jacobian
@@ -57,7 +59,7 @@ class CKinematics
* Compute the arm Jacobian
*
*/
- static Eigen::MatrixXd ArmJac(CArm& arm);
+ static Eigen::MatrixXd ArmJac(UAM::CArm& arm);
/**
* \brief Compute Quadrotor Jacobian
@@ -73,7 +75,9 @@ class CKinematics
* Compute the UAM Jacobian and the position of the quadrotor
*
*/
- static void UAMKine(const CQuad& quad, CArm& arm, CHT& HT, CUAM& uam);
+ static void UAMKine(const UAM::CQuad& quad, UAM::CArm& arm, UAM::CHT& HT, UAM::CUAM& uam);
};
+} // End of UAM namespace
+
#endif
diff --git a/src/uam_task_ctrl.cpp b/src/uam_task_ctrl.cpp
index c69296524933104394747efff3598ad76aac9f98..4e971e58dc5e42244f5e8281ed93d7f308c26a38 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_ = CCommon_Fc::get_datetime();
+ // this->datetime_ = UAM::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,
- CHT& HT,
- CArm& arm,
- const CQuad& quad,
- CCtrlParams& ctrl_params,
+ UAM::CHT& HT,
+ UAM::CArm& arm,
+ const UAM::CQuad& quad,
+ UAM::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;
- CKinematics::UAMKine(this->quad_,this->arm_,this->T_,this->uam_);
+ UAM::CKinematics::UAMKine(this->quad_,this->arm_,this->T_,this->uam_);
uam_hierarchical_ctrl();
// Arm positions increment
@@ -97,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 = CCommon_Fc::CalcPinv(JIR_VS);
+ MatrixXd JIR_VS_pseudo = UAM::CCommon_Fc::CalcPinv(JIR_VS);
NIR_VS = (eye-(JIR_VS_pseudo*JIR_VS));
// task Jacobian and sigma
@@ -117,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 = CCommon_Fc::CalcPinv(JIR_VS_G);
+ MatrixXd JIR_VS_G_pseudo = UAM::CCommon_Fc::CalcPinv(JIR_VS_G);
// Third task after secondary aligning CoG
NIR_VS_G = (eye-(JIR_VS_G_pseudo*JIR_VS_G));
@@ -230,11 +230,11 @@ void CQuadarm_Task_Priority_Ctrl::task_infrad(MatrixXd& JIR,MatrixXd& JIR_pseudo
}
// }
- MatrixXd Hinv = CCommon_Fc::CalcPinv(H);
+ MatrixXd Hinv = UAM::CCommon_Fc::CalcPinv(H);
// weighting matrix
MatrixXd temp = JIR*Hinv*JIR.transpose();
- JIR_pseudo = Hinv*JIR.transpose()*CCommon_Fc::CalcPinv(temp);
+ JIR_pseudo = Hinv*JIR.transpose()*UAM::CCommon_Fc::CalcPinv(temp);
// cout << "xxxxxx Task velocities xxxxxx" << endl;
// cout << JIR_pseudo*sigmaIR << endl;
@@ -258,9 +258,9 @@ void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,Mat
JVS = Jqa1;
// task jacobian pseudo inverse
- //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_);
+ //JVS_pseudo = UAM::CCommon_Fc::CalcPinv(Jqa1);
+ // JVS_pseudo = UAM::CCommon_Fc::WeightInvJac(Jqa1);
+ JVS_pseudo = UAM::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;
@@ -322,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) = CCommon_Fc::GetTransform(joint_pose.at(jj));
+ Transf.at(jj) = UAM::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
@@ -423,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 = CCommon_Fc::CalcPinv(JG);
+ JG_pseudo = UAM::CCommon_Fc::CalcPinv(JG);
sigmaG = -(this->ctrl_params_.cog_PGxy.transpose()*this->ctrl_params_.cog_PGxy);
}
@@ -453,6 +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 = CCommon_Fc::CalcPinv(JL);
+ JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL);
}
diff --git a/src/uam_task_ctrl.h b/src/uam_task_ctrl.h
index e9aedd8c79a96cf543042accde5bf8e6c0e76f1f..91efd72a31de5ce4f696da54c04dbfa595f519ce 100644
--- a/src/uam_task_ctrl.h
+++ b/src/uam_task_ctrl.h
@@ -43,35 +43,17 @@ typedef struct{
double target_dist; // Target distance
}goal_obj;
-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;
-}cog_data;
-
-typedef struct{
- VectorXd vs;
- VectorXd cog;
- VectorXd jl;
-}eig_values;
-
-typedef struct{
- MatrixXd ir;
- MatrixXd vs;
- MatrixXd cog;
- MatrixXd jl;
-}jacobians;
-
class CQuadarm_Task_Priority_Ctrl
{
private:
- CUAM uam_; // Unmanned Aerial Manipulator.
- CArm arm_; // Arm.
- CQuad quad_; // Quadrotor.
+ UAM::CUAM uam_; // Unmanned Aerial Manipulator.
+ UAM::CArm arm_; // Arm.
+ UAM::CQuad quad_; // Quadrotor.
- CCtrlParams ctrl_params_; // Control Law parameters.
+ UAM::CCtrlParams ctrl_params_; // Control Law parameters.
- CHT T_; // Homogeneous Transforms.
+ UAM::CHT T_; // Homogeneous Transforms.
MatrixXd cam_vel_; // Camera velocities.
@@ -130,11 +112,11 @@ class CQuadarm_Task_Priority_Ctrl
//TODO: these variables are public for debugging purposes
// they must be private and set get functions created.
- cog_data arm_cog_data_; // Arm CoG data to debug in higher levels.
+ UAM::ArmCogData arm_cog_data_; // Arm CoG data to debug in higher levels.
- eig_values eig_values_; // Task jacobian eigenvalues.
+ UAM::TaskEigVal eig_values_; // Task jacobian eigenvalues.
- jacobians jacobians_; // Task Jacobians.
+ UAM::TaskJac jacobians_; // Task Jacobians.
/**
* \brief Constructor
@@ -160,10 +142,10 @@ class CQuadarm_Task_Priority_Ctrl
*/
void quadarm_task_priority_ctrl(const MatrixXd& quad_dist_obs,
const goal_obj& goal,
- CHT& HT,
- CArm& arm,
- const CQuad& quad,
- CCtrlParams& CtrlParams,
+ UAM::CHT& HT,
+ UAM::CArm& arm,
+ const UAM::CQuad& quad,
+ UAM::CCtrlParams& CtrlParams,
MatrixXd& robot_pos,
MatrixXd& robot_vel);
};