Skip to content
Snippets Groups Projects
Commit e0b1b06e authored by asantamaria's avatar asantamaria
Browse files

Added namespace UAM

parent cec25cd6
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/common_fc.cpp
+ 4
− 0
View file @ e0b1b06e
#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
src/common_fc.h
+ 3
− 4
View file @ e0b1b06e
......@@ -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
src/common_obj.h
+ 69
− 44
View file @ e0b1b06e
......@@ -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
src/kinematics.cpp
+ 7
− 4
View file @ e0b1b06e
#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
src/kinematics.h
+ 7
− 3
View file @ e0b1b06e
......@@ -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
src/uam_task_ctrl.cpp
+ 16
− 16
View file @ e0b1b06e
......@@ -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);
}
src/uam_task_ctrl.h
+ 12
− 30
View file @ e0b1b06e
......@@ -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);
};
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment