diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index 54d370e8c48d3de7eb22e37e5139425e9a15a028..44205c5a00fdcec6348126c25bb35a1680da6ca1 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -218,19 +218,18 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
// Null space projector
MatrixXd eye = MatrixXd::Identity(4+this->arm_.nj,4+this->arm_.nj);
- MatrixXd Nqa1 = (eye-(JIR_pseudo*JIR));
+ MatrixXd NIR = (eye-(JIR_pseudo*JIR));
// task Jacobian and sigma
MatrixXd JVS,JVS_wpseudo,sigmaVS;
task_vs(JVS,JVS_wpseudo,sigmaVS);
// task velocity
- this->ctrl_params_.vs_vel = Nqa1 * JVS_wpseudo * sigmaVS;
+ this->ctrl_params_.vs_vel = NIR * JVS_wpseudo * sigmaVS;
// TASK 2: CoG alignement ______________________________________
// Null space projector
- //MatrixXd Nqa1 = (eye-(JVS_wpseudo*JVS));
MatrixXd JIR_VS = MatrixXd::Zero(JIR.rows()+JVS.rows(),JVS.cols());
JIR_VS.block(0,0,JIR.rows(),JIR.cols()) = JIR;
JIR_VS.block(JIR.rows()-1,0,JVS.rows(),JVS.cols())=JVS;
@@ -244,9 +243,7 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
// Gain
double lambdaG = this->ctrl_params_.cog_gain;
-
// task velocity
- // this->ctrl_params_.cog_vel = Nqa1 * JG_pseudo * lambdaG * sigmaG;
this->ctrl_params_.cog_vel = NIR_VS * JG_pseudo * lambdaG * sigmaG;
// TASK 3: Joint limits ________________________
@@ -258,7 +255,6 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
MatrixXd NIR_VS_G = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
MatrixXd JIR_VS_G_pseudo = calcPinv(JIR_VS_G);
-
// Third task after secondary aligning CoG
NIR_VS_G = (eye-(JIR_VS_G_pseudo*JIR_VS_G));
@@ -277,12 +273,13 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
//******************************************************************
// Weighted sum of subtasks
- // this->ctrl_params_.cog_vel = Nqa1 * (JG_pseudo * lambdaG * sigmaG + JL_pseudo * lambdaL * sigmaL);
+ // this->ctrl_params_.cog_vel = NIR * (JG_pseudo * lambdaG * sigmaG + JL_pseudo * lambdaL * sigmaL);
// this->ctrl_params_.jntlim_vel = MatrixXd::Zero(9,1);
//******************************************************************
//Total velocity __________________________________________
- if (!this->ctrl_params_.enable_sec_task) {
+ if (!this->ctrl_params_.enable_sec_task)
+ {
this->ctrl_params_.cog_vel = MatrixXd::Zero(4+this->arm_.nj,1);
this->ctrl_params_.jntlim_vel = MatrixXd::Zero(4+this->arm_.nj,1);
cout << "[Task Priority Control]: Secondary tasks not enabled" << endl;
@@ -299,11 +296,9 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
Vtotal = this->ctrl_params_.lambda_robot.array()*Vtotal.array();
- // Check a singular configuration /////
+ // Check singular configurations
if (isnan(Vtotal(0,0)))
- {
Vtotal=MatrixXd::Zero(4+this->arm_.nj,1);
- }
// Rearranging terms
this->uam_.vel = MatrixXd::Zero(6+this->arm_.nj,1);
@@ -375,49 +370,43 @@ void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,Mat
}
void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,MatrixXd& sigmaG)
{
- // Initializations
- this->arm_.mass = 0; // Total arm mass
- MatrixXd mass = MatrixXd::Zero(this->arm_.nj,1); // Mass of the links
- this->ctrl_params_.cog_arm = MatrixXd::Zero(3,1); // Arm CoG
- MatrixXd arm_cg = MatrixXd::Zero(4,1); // Sum of CoG vector
+ // KDL stuff
vector<Frame> joint_pose(this->arm_.nj); // Vector of Joint frames
- joint_pose.reserve(this->arm_.nj);
+ joint_pose.resize(this->arm_.nj);
vector<Matrix4d> Transf(this->arm_.nj); // Vector of HT of frames relative to each links
- Transf.reserve(this->arm_.nj);
+ Transf.resize(this->arm_.nj);
+
+ // Eigen stuff
vector<MatrixXd> T_base_to_joint(this->arm_.nj+1); // Vector of HT of frame links relative to base
- T_base_to_joint.reserve(this->arm_.nj+1);
+ T_base_to_joint.resize(this->arm_.nj+1);
vector<MatrixXd> IIIrdcolRot_b_x(this->arm_.nj+1); // Vector of r3d column of HT
- IIIrdcolRot_b_x.reserve(this->arm_.nj+1);
- vector<MatrixXd> frame_link(this->arm_.nj+1); // Origin of each link's frame
- frame_link.reserve(this->arm_.nj+1);
+ IIIrdcolRot_b_x.resize(this->arm_.nj+1);
+ vector<MatrixXd> link_origin(this->arm_.nj+1); // Origin of each link's frame
+ link_origin.resize(this->arm_.nj+1);
- vector<MatrixXd> dist(this->arm_.nj); // Vector of relative distance of each link to its CoGlink
- dist.reserve(this->arm_.nj);
+ // CoG data
+ this->arm_.mass = 0; // Total arm mass
+ MatrixXd arm_cg = MatrixXd::Zero(4,1); // Sum of CoG vector
+ this->arm_cog_data_.arm_cog = MatrixXd::Zero(3,1); // Arm CoG
+ this->arm_cog_data_.link_cog.resize(this->arm_.nj);
vector<MatrixXd> link_cg(this->arm_.nj); // Vector of link CoG relative to base
- link_cg.reserve(this->arm_.nj);
+ link_cg.resize(this->arm_.nj);
// Get arm segments
- unsigned int js = this->arm_.chain.getNrOfSegments();
- vector<Segment> arm_segment(js);
- arm_segment.reserve(js);
- for (unsigned int ii = 1; ii < js; ++ii)
+ unsigned int num_seg = this->arm_.chain.getNrOfSegments();
+ vector<Segment> arm_segment(num_seg);
+ arm_segment.resize(num_seg);
+ for (unsigned int ii = 1; ii < num_seg; ++ii)
arm_segment.at(ii) = this->arm_.chain.getSegment(ii);
- // Origin of the base frame
- MatrixXd pini(4,1);
- pini.col(0) << 0,0,0,1;
-
// Initial transform
T_base_to_joint.at(0) = this->T_.armbase_in_baselink;
- // T_base_to_joint.at(0) = Matrix4d::Identity();
-
// 3rd column of rotation matrix
IIIrdcolRot_b_x.at(0) = T_base_to_joint.at(0).block(0,2,3,1);
// Origin of the initial frame
- frame_link.at(0) = T_base_to_joint.at(0) * pini;
+ link_origin.at(0) = T_base_to_joint.at(0).col(3);
// Debug
- this->arm_cog_data_.link.resize(this->arm_.nj);
this->arm_.T_base_to_joint.resize(this->arm_.nj+1);
this->arm_.T_base_to_joint.at(0) = T_base_to_joint.at(0);
@@ -430,37 +419,35 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
for (unsigned int jj = 0; jj < this->arm_.nj; ++jj)
{
// Get mass of each link
- mass(jj,0) = arm_segment.at(jj+1).getInertia().getMass();
+ this->arm_.joint_info.at(jj).mass = arm_segment.at(jj+1).getInertia().getMass();
// 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));
-
// HT r.t. base link
T_base_to_joint.at(jj+1) = T_base_to_joint.at(jj) * Transf.at(jj);
-
-//TODO: Debugging till here
-
// 3rd column of rotation matrix
IIIrdcolRot_b_x.at(jj+1) = T_base_to_joint.at(jj+1).block(0,2,3,1);
// Origin of the link's frame r.t. base_link
- frame_link.at(jj+1) = T_base_to_joint.at(jj+1) * pini;
+ link_origin.at(jj+1) = T_base_to_joint.at(jj+1).col(3);
// Distance to the middle of the link jj
- dist.at(jj) = (frame_link.at(jj+1)-frame_link.at(jj))/2;
+ MatrixXd link_mid = (link_origin.at(jj+1)-link_origin.at(jj))/2;
+ // Link CoG coordinates
+ this->arm_cog_data_.link_cog.at(jj) = link_origin.at(jj)+link_mid;
// gravity for the link jj
- link_cg.at(jj) = mass(jj,0)*(frame_link.at(jj)+dist.at(jj));
+ link_cg.at(jj) = this->arm_.joint_info.at(jj).mass * this->arm_cog_data_.link_cog.at(jj);
// Sum of link CoGs
arm_cg = arm_cg + link_cg.at(jj);
// Sum of link mass
- this->arm_.mass = this->arm_.mass + mass(jj,0);
+ this->arm_.mass = this->arm_.mass + this->arm_.joint_info.at(jj).mass;
//Debug
// std::cout << "num segments: " << arm_segment.size() << std::endl;
// std::cout << "Link: " << jj << " name:";
// std::cout << arm_segment.at(jj+1).getName();
- // std::cout << " mass: " << mass(jj,0);
+ // std::cout << " mass: " << this->arm_.joint_info.at(jj).mass;
// std::cout << " link_cog:" << std::endl;
- // std::cout << frame_link.at(jj)+dist.at(jj) << std::endl;
+ // std::cout << link_origin.at(jj)+this->arm_cog_data_.cog_joint.at(jj) << std::endl;
// std::cout << "Transform: " << std::endl;
// std::cout << T_base_to_joint.at(jj+1) <<std::endl;
@@ -476,58 +463,44 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
// std::cout << Rot.eulerAngles(0,1,2) << std::endl;
// Fill link information
- this->arm_cog_data_.link.at(jj).origin = T_base_to_joint.at(jj+1);
- this->arm_cog_data_.link.at(jj).cog = (frame_link.at(jj+1)-frame_link.at(jj))/2;
- this->arm_cog_data_.link.at(jj).mass = mass(jj,0);
this->arm_.T_base_to_joint.at(jj+1) = T_base_to_joint.at(jj+1);
}
// vector from arm base to CoG
- this->ctrl_params_.cog_arm = arm_cg/this->arm_.mass;
+ this->arm_cog_data_.arm_cog = arm_cg/this->arm_.mass;
// Debug
// std::cout << "mass: " << this->arm_.mass << std::endl;
// std::cout << "arm_cg: " << arm_cg << std::endl;
- // std::cout << "cog: " << this->ctrl_params_.cog_arm << std::endl;
-
- // Fill arm CoG information
- this->arm_cog_data_.arm_mass = this->arm_.mass;
- this->arm_cog_data_.arm_cog = this->ctrl_params_.cog_arm;
-
+ // std::cout << "cog: " << this->arm_cog_data_.arm_cog << std::endl;
// Rotation between quadrotor body and inertial frames
+ // TODO: If attitude is estimated, then it could be roll and pitch
Matrix3d Rib;
// Rib = AngleAxisd(0, Vector3d::UnitZ()) * AngleAxisd(this->ctrl_params_.q_rollpitch(1,0), Vector3d::UnitY()) * AngleAxisd(this->ctrl_params_.q_rollpitch(0,0), Vector3d::UnitX());
Rib = MatrixXd::Identity(3,3);
// X and Y values of CoG in quadrotor inertial frame
- MatrixXd cog_arm_in_qi = Rib * this->ctrl_params_.cog_arm.block(0,0,3,1);
-
- // This change is done considering arm base frame orientation
- // TODO: Work with quadrotor orientation
- //std::cout << "******" << std::endl << cog_arm_in_qi << std::endl;
+ MatrixXd cog_arm_in_qi = Rib * this->arm_cog_data_.arm_cog.block(0,0,3,1);
this->ctrl_params_.cog_PGxy = cog_arm_in_qi.block(0,0,2,1);
- //this->ctrl_params_.cog_PGxy = MatrixXd::Zero(2,1);
- //this->ctrl_params_.cog_PGxy(0,0) = cog_arm_in_qi(2,0);
- //this->ctrl_params_.cog_PGxy(1,0) = cog_arm_in_qi(1,0);
- MatrixXd X_partial;
- // Partial jacobian of each link (augmented links)
+ // Partial CoG and jacobian of each link (augmented links: from current to end-effector)
+ MatrixXd CoG_partial;
MatrixXd Jj_cog(3,this->arm_.nj);
for (unsigned int jj = 0; jj < this->arm_.nj; ++jj)
{
+ // Get the center of gravity from the current link to the end effector
double partial_mass = 0;
MatrixXd partial_arm_cg = MatrixXd::Zero(4,1);
-
for (unsigned int ii = jj; ii < this->arm_.nj; ++ii)
{
- partial_mass = partial_mass + mass(ii,0);
+ partial_mass = partial_mass + this->arm_.joint_info.at(ii).mass;
partial_arm_cg = partial_arm_cg + link_cg.at(ii);
}
if(partial_mass!=0){
- X_partial = partial_arm_cg/partial_mass;
+ CoG_partial = partial_arm_cg/partial_mass;
Matrix3d screw_rot=Matrix3d::Zero();
screw_rot(0,1) = -IIIrdcolRot_b_x.at(jj)(2);
@@ -537,7 +510,7 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
screw_rot(2,0) = -IIIrdcolRot_b_x.at(jj)(1);
screw_rot(2,1) = IIIrdcolRot_b_x.at(jj)(0);
- Jj_cog.col(jj) = (partial_mass/this->arm_.mass)*screw_rot*X_partial.block(0,0,3,1);
+ Jj_cog.col(jj) = (partial_mass/this->arm_.mass)*screw_rot*CoG_partial.block(0,0,3,1);
}
else Jj_cog.col(jj) = MatrixXd::Zero(3,1);
}
@@ -546,9 +519,7 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
JG.block(0,0,1,4)=MatrixXd::Zero(1,4);
MatrixXd Jacob_temp(3,this->arm_.nj);
for (unsigned int ii = 0; ii < this->arm_.nj; ++ii)
- {
Jacob_temp.col(ii) = Rib * Jj_cog.col(ii);
- }
JG.block(0,4,1,this->arm_.nj)=2*this->ctrl_params_.cog_PGxy.transpose()*Jacob_temp.block(0,0,2,this->arm_.nj);
diff --git a/src/quadarm_task_priority_ctrl.h b/src/quadarm_task_priority_ctrl.h
index 91f797edb16660f8d1afa822a04bf3177e676e19..da9688720ffc21e1673251b105e79eb12ffa9afa 100644
--- a/src/quadarm_task_priority_ctrl.h
+++ b/src/quadarm_task_priority_ctrl.h
@@ -57,6 +57,7 @@ typedef struct{
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
@@ -108,14 +109,7 @@ typedef struct{
}ctrl_params;
typedef struct{
- MatrixXd origin; // Origin coordinates
- MatrixXd cog; // CoG coordinates (3D vector) w.r.t. link origin
- double mass; // Mass
-}link_obj;
-
-typedef struct{
- vector<link_obj> link; // Vector of link CoG relative to base
- double arm_mass; // Arm Mass;
+ 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;