diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp index 6d842bf7d60b2c970a3833d14824dbc4deb31d42..b97ccaa46fe416dc8d27233ca1e9974f92d3f5e1 100644 --- a/src/quadarm_task_priority_ctrl.cpp +++ b/src/quadarm_task_priority_ctrl.cpp @@ -13,13 +13,7 @@ CQuadarm_Task_Priority_Ctrl::~CQuadarm_Task_Priority_Ctrl() { } -/*void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const goal_obj& goal, - const ht& HT, - const Chain& kdl_chain, - const vector<arm_joint> joint_info, - ctrl_params& ctrl_params, - MatrixXd& joint_pos, - MatrixXd& robot_vels)*/ +// Main public function void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const goal_obj& goal, const ht& HT, const arm& arm, @@ -57,6 +51,7 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const goal_obj& goa robot_vel = this->vel_.quadarm; } +// Kinematics void CQuadarm_Task_Priority_Ctrl::qa_kinematics(){ // Get arm Homogenous transform, arm tip in base_link @@ -71,6 +66,7 @@ void CQuadarm_Task_Priority_Ctrl::qa_kinematics(){ // Quadrotor Jacobian body to inertial frames MatrixXd Jb2i = quadrotor_jacobian(); + // Inertial to body frames MatrixXd Ji2b = calcPinv(Jb2i); @@ -103,63 +99,6 @@ void CQuadarm_Task_Priority_Ctrl::qa_kinematics(){ this->Jqa_.block(0,0,6,6) = Rot*Jb2c*Ji2b; this->Jqa_.block(0,6,6,this->arm_.nj)=Rot*Ja; } - -MatrixXd CQuadarm_Task_Priority_Ctrl::quadrotor_jacobian() -{ - double psi,theta,phi; - - phi = this->arm_.jnt_pos(3,0); - theta = this->arm_.jnt_pos(4,0); - // psi = this->arm_.jnt_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; -} - -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; -} - Matrix4d CQuadarm_Task_Priority_Ctrl::arm_f_kinematics() { @@ -198,7 +137,62 @@ Matrix4d CQuadarm_Task_Priority_Ctrl::arm_f_kinematics() 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->arm_.jnt_pos(3,0); + theta = this->arm_.jnt_pos(4,0); + // psi = this->arm_.jnt_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::qa_control(){ // Underactuated Quadrotor @@ -263,7 +257,7 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){ this->ctrl_params_.jntlim_vel = Nqa1_G * JL_pseudo * lambdaL * sigmaL; //****************************************************************** - + // Weighted sum of subtasks // this->ctrl_params_.cog_vel = Nqa1 * (JG_pseudo * lambdaG * sigmaG + JL_pseudo * lambdaL * sigmaL); // this->ctrl_params_.jntlim_vel = MatrixXd::Zero(9,1); @@ -288,36 +282,6 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){ this->vel_.quadarm.block(0,0,3,1) = Vtotal.block(0,0,3,1); this->vel_.quadarm.block(5,0,1+this->arm_.nj,1) = Vtotal.block(3,0,1+this->arm_.nj,1); } - -void CQuadarm_Task_Priority_Ctrl::weight_inverse(const MatrixXd& J, MatrixXd& J_inverse) -{ - - 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); - // double alpha = (0.5*alpha_min) + (0.5*alpha_min*tanh(tmp)); - - MatrixXd W = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj); - - for (unsigned int ii = 0; ii < 4+this->arm_.nj; ++ii) - { - // Quadrotor values - if(ii<4) - W(ii,ii) = 1-alpha; - // Arm values - else - W(ii,ii) = alpha; - } - - MatrixXd temp = J*W.inverse()*J.transpose(); - - J_inverse = W.inverse()*J.transpose()*temp.inverse(); -} - void CQuadarm_Task_Priority_Ctrl::sec_task_cog(MatrixXd& JG,MatrixXd& sigmaG) { // Initializations @@ -374,7 +338,6 @@ void CQuadarm_Task_Priority_Ctrl::sec_task_cog(MatrixXd& JG,MatrixXd& sigmaG) 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(); // Joint frames @@ -407,7 +370,15 @@ void CQuadarm_Task_Priority_Ctrl::sec_task_cog(MatrixXd& JG,MatrixXd& sigmaG) // 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->ctrl_params_.cog_PGxy = cog_arm_in_qi.block(0,0,2,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; + //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) @@ -451,7 +422,6 @@ void CQuadarm_Task_Priority_Ctrl::sec_task_cog(MatrixXd& JG,MatrixXd& sigmaG) sigmaG = -(this->ctrl_params_.cog_PGxy.transpose()*this->ctrl_params_.cog_PGxy); } - void CQuadarm_Task_Priority_Ctrl::sec_task_jl(MatrixXd& JL,MatrixXd& sigmaL) { // Get diagonal gain matrix @@ -479,288 +449,35 @@ void CQuadarm_Task_Priority_Ctrl::sec_task_jl(MatrixXd& JL,MatrixXd& sigmaL) JL.block(0,4,1,this->arm_.nj) = -2*Jtemp.transpose(); } -void CQuadarm_Task_Priority_Ctrl::old_sec_task_vel_unina(){ - - double ndof = 4+this->arm_.nj; - this->vel_.old_quadarm= MatrixXd::Zero(ndof,1); - - // Kinematic Stabilization - MatrixXd w_stabil = old_sec_task_stabil(); - this->vel_.old_quadarm = this->vel_.old_quadarm + this->ctrl_params_.stabil_gain*w_stabil; - - // Center of Gravity alignement - MatrixXd w_cg = old_sec_task_cg(); - this->vel_.old_quadarm.block(4,0,5,1) = this->vel_.old_quadarm.block(4,0,5,1) + this->ctrl_params_.cog_gain*w_cg; - - // Joint limits - MatrixXd w_jointlimits = old_sec_task_jointlimits(); - this->vel_.old_quadarm.block(4,0,5,1) = this->vel_.old_quadarm.block(4,0,5,1) + this->ctrl_params_.jntlim_gain*w_jointlimits; - -} - -MatrixXd CQuadarm_Task_Priority_Ctrl::old_sec_task_stabil(){ - - // // Stabilize quadrotor /////////////////////////////////////////////// - MatrixXd w_stabil = MatrixXd::Zero(4+this->arm_.nj,1); - - float dq = 0.001; - - for (unsigned int i = 0; i < this->arm_.nj; ++i) - { - MatrixXd q_old(this->arm_.nj,1),q_new(this->arm_.nj,1); - MatrixXd angles_old(3,1),angles_new(3,1); - - q_old = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1); - q_old(i,0) = q_old(i,0)-dq; - q_new = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1); - q_new(i,0) = q_new(i,0)+dq; - - angles_old = old_get_stabil_angles(q_old); - angles_new = old_get_stabil_angles(q_new); - - w_stabil(i,0)=(((angles_new(0)*angles_new(0))+(angles_new(1)*angles_new(1)))-((angles_old(0)*angles_old(0))+(angles_old(1)*angles_old(1))))/dq; - - } - - return w_stabil; -} - -MatrixXd CQuadarm_Task_Priority_Ctrl::old_get_stabil_angles(const MatrixXd &qa){ - - for (unsigned int jj = 0; jj < this->arm_.nj; ++jj){ - this->arm_.jnt_pos_kdl.data(jj) = qa(jj,0); - } - - // compute Cartesian pose in realtime - Frame kdlTarm; - this->arm_.jnt_to_pose_solver->JntToCart(this->arm_.jnt_pos_kdl, kdlTarm); - double ro,pit,ya; - kdlTarm.M.GetEulerZYX(ya,pit,ro); - - Matrix4d HT,inv_HT; - Matrix3d Rarm; - // euler convention zyx - Rarm = Eigen::AngleAxisd(ya, Vector3d::UnitZ()) * Eigen::AngleAxisd(pit, Vector3d::UnitY()) * Eigen::AngleAxisd(ro, Vector3d::UnitX()); - HT.block(0,0,3,3) = Rarm; - HT(0,3) = (double)kdlTarm.p.data[0]; - HT(1,3) = (double)kdlTarm.p.data[1]; - HT(2,3) = (double)kdlTarm.p.data[2]; - HT.row(3) << 0, 0, 0, 1; - - // inv_HT = this->T0c_new_*HT.inverse(); - inv_HT = HT.inverse(); - - MatrixXd angles(3,1); - Matrix3d Rot; - Rot = inv_HT.block(0,0,3,3); - angles = Rot.eulerAngles(2,1,0); - - return angles; -} - - -MatrixXd CQuadarm_Task_Priority_Ctrl::old_sec_task_cg(){ - - double ndof = 4+this->arm_.nj; - MatrixXd w_cg = MatrixXd::Zero(this->arm_.nj,1); - this->vel_.old_quadarm= MatrixXd::Zero(ndof,1); - - // Get arm segments - // num of 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) - { - arm_segment.at(ii) = this->arm_.chain.getSegment(ii); - } - - double dq=0.01; - - MatrixXd qa_old(this->arm_.nj,1),qa_new(this->arm_.nj,1); - - for (unsigned int ii = 0; ii < this->arm_.nj; ++ii){ - - qa_old = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1); - qa_new = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1); - qa_old(ii,0) = qa_old(ii,0)-dq; - qa_new(ii,0) = qa_new(ii,0)+dq; - - MatrixXd rcm_old = old_get_cg_dist(qa_old,arm_segment); - MatrixXd rcm_new = old_get_cg_dist(qa_new,arm_segment); - - w_cg(ii,0)=-(((rcm_new(0,0)*rcm_new(0,0)+rcm_new(1,0)*rcm_new(1,0))-(rcm_old(0,0)*rcm_old(0,0)+rcm_old(1,0)*rcm_old(1,0)))/(2*dq)); - } - - return w_cg; -} - -MatrixXd CQuadarm_Task_Priority_Ctrl::old_get_cg_dist(const MatrixXd &qa,const vector<Segment> &arm_segment){ - - - MatrixXd rcm = MatrixXd::Zero(3,1); - - vector<Frame> joint_pose(this->arm_.nj); - vector<Matrix4d> Transf(this->arm_.nj); - vector<MatrixXd> T_base_to_joint(this->arm_.nj+1); - vector<MatrixXd> dist(this->arm_.nj); - vector<MatrixXd> link_cg(this->arm_.nj); - // mass of each link - MatrixXd mass = MatrixXd::Zero(this->arm_.nj,1); - - joint_pose.reserve(this->arm_.nj); - Transf.reserve(this->arm_.nj); - T_base_to_joint.reserve(this->arm_.nj+1); - dist.reserve(this->arm_.nj); - link_cg.reserve(this->arm_.nj); - - double arm_mass = 0; - MatrixXd arm_cg = MatrixXd::Zero(4,1); - - // Get Homogenous transforms of the joints - for (unsigned int jj = 0; jj < this->arm_.nj; ++jj) - { - joint_pose.at(jj) = arm_segment.at(jj+1).pose(qa(jj,0)); - Transf.at(jj) = GetTransform(joint_pose.at(jj)); - mass(jj,0) = arm_segment.at(jj+1).getInertia().getMass(); - } - - - MatrixXd pini(4,1); - pini.col(0) << 0,0,0,1; - - //Fixed transform between base_link and arm_base - Matrix4d Tbase = Matrix4d::Zero(); - Tbase(0,2) = 1; - Tbase(1,1) = 1; - Tbase(2,0) = -1; - Tbase(3,3) = 1; - - T_base_to_joint.at(0) = Tbase*pini; - T_base_to_joint.at(1) = Tbase*Transf.at(0)*pini; - T_base_to_joint.at(2) = Tbase*Transf.at(0)*Transf.at(1)*pini; - T_base_to_joint.at(3) = Tbase*Transf.at(0)*Transf.at(1)*Transf.at(2)*pini; - T_base_to_joint.at(4) = Tbase*Transf.at(0)*Transf.at(1)*Transf.at(2)*Transf.at(3)*pini; - T_base_to_joint.at(5) = Tbase*Transf.at(0)*Transf.at(1)*Transf.at(2)*Transf.at(3)*Transf.at(4)*pini; - - for (unsigned int jj = 0; jj < this->arm_.nj; ++jj) - { - // Distance to the middle of the link jj - dist.at(jj) = (T_base_to_joint.at(jj+1)-T_base_to_joint.at(jj))/2; - - // gravity for the link jj - link_cg.at(jj) = mass(jj,0)*(T_base_to_joint.at(jj)+dist.at(jj)); - - // arm CoG - arm_cg = arm_cg + link_cg.at(jj); - - // arm mass - arm_mass = arm_mass + mass(jj,0); - } - - // vector from arm base to CoG - rcm = arm_cg/arm_mass; - - // ************************************************************* - - // // CoG r.t. quadrotor body frame - // MatrixXd rcm_qb(4,1); - // rcm_qb = Tbase.inverse() * rcm; - - // // Rotation between quadrotor body and inertial frames - // Matrix3d Rot; - // Rot = AngleAxisd(this->ctrl_params_.q_rollpitch(0,0), Vector3d::UnitZ()) * AngleAxisd(this->ctrl_params_.q_rollpitch(1,0), Vector3d::UnitY()) * AngleAxisd(0, Vector3d::UnitX()); - - // MatrixXd rcm_2(3,1),rcm_3(3,1), mat1(3,1),mat2(3,1),mat3(3,1), Q1(3,1), Q2(3,1); - - // // CoG in quadrotor inertial frame - // rcm_2 = Rot.transpose()*rcm_qb.block(0,0,3,1); - - // // distance of the CoG to the vertical centered line of the quadrotor frame. - // Vector3d Qs(3,1), vec1(3,1), vec2(3,1); - - // Q1 = MatrixXd::Zero(3,1); - // Q2 = MatrixXd::Zero(3,1); - // Q1(2,0) = -0.1; - // Q2(2,0) = -0.5; - - // vec1 = rcm_2 - Q1; - // vec2 = rcm_2 - Q2; - // Qs = Q2 - Q1; - - // mat1 = vec1.cross(vec2); - // mat2 = mat1.array().abs(); - // mat3 = Qs.array().abs(); - - // cout << "11" << endl; - // cout << rcm_2 << endl; - // cout << "22" << endl; - // cout << mat3 << endl; - - - // for (unsigned int ii = 0; ii < 3; ++ii) - // { - // if (mat3(ii,0)==0) { - // rcm_3(ii,0)=0; - // } - // else { - // cout << "yeyeyeye" << endl; - // rcm_3(ii,0) = mat2(ii,0)/mat3(ii,0); - // } - // } - - // rcm.block(0,0,3,1) = rcm_3; - - // cout << "33" << endl; - // cout << rcm << endl; - - // ************************************************************* - - return rcm; -} - -MatrixXd CQuadarm_Task_Priority_Ctrl::old_sec_task_jointlimits(){ - // Joint Limits ////////////////////////////////////////////////////77 - - MatrixXd w_jointlimits = MatrixXd::Zero(this->arm_.nj,1); - - MatrixXd joint_optim(this->arm_.nj,1), joint_max(this->arm_.nj,1), joint_min(this->arm_.nj,1); +// MISC functions +void CQuadarm_Task_Priority_Ctrl::weight_inverse(const MatrixXd& J, MatrixXd& J_inverse) +{ - joint_max(0,0) = 0.548; - joint_min(0,0) = -0.548; - - joint_max(1,0) = 1.57; - joint_min(1,0) = -1.57; + 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; - joint_max(2,0) = 5.2; - joint_min(2,0) = 0; + double tmp = 2*3.14159*((this->target_dist_-w_min)/(w_max-w_min))-3.14159; - joint_max(3,0) = 5.2; - joint_min(3,0) = 0; + double alpha = (0.5*(1+alpha_min)) + (0.5*(1-alpha_min))*tanh(tmp); + // double alpha = (0.5*alpha_min) + (0.5*alpha_min*tanh(tmp)); - joint_max(4,0) = 3.1416; - joint_min(4,0) = -3.1416; + MatrixXd W = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj); - for (unsigned int jj = 0; jj < this->arm_.nj; ++jj) + for (unsigned int ii = 0; ii < 4+this->arm_.nj; ++ii) { - joint_optim(jj,0) = (joint_max(jj,0)+joint_min(jj,0))/2; - - double sq_pos = this->arm_.nj * (joint_max(jj,0)-joint_min(jj,0)); - - w_jointlimits(jj,0)=-((this->arm_.jnt_pos(6+jj,0)-joint_optim(jj,0))/sq_pos); + // Quadrotor values + if(ii<4) + W(ii,ii) = 1-alpha; + // Arm values + else + W(ii,ii) = alpha; } - return w_jointlimits; -} + MatrixXd temp = J*W.inverse()*J.transpose(); -void CQuadarm_Task_Priority_Ctrl::old_sec_task_vel_catec() -{ - //DOF's - double ndof = 4+this->arm_.nj; - this->vel_.old_quadarm = MatrixXd::Zero(ndof,1); + J_inverse = W.inverse()*J.transpose()*temp.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 @@ -804,7 +521,6 @@ MatrixXd CQuadarm_Task_Priority_Ctrl::calcPinv(const MatrixXd &a){ return m_pinv; } - Matrix4d CQuadarm_Task_Priority_Ctrl::GetTransform(const Frame &f) { @@ -826,7 +542,6 @@ Matrix4d CQuadarm_Task_Priority_Ctrl::GetTransform(const Frame &f) return T; } - 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 diff --git a/src/quadarm_task_priority_ctrl.h b/src/quadarm_task_priority_ctrl.h index a26e7ba75edc46e114d56e22a92d612d1debf51a..a9d04217ec93817aca7579d5574e8d7a7dc178d8 100644 --- a/src/quadarm_task_priority_ctrl.h +++ b/src/quadarm_task_priority_ctrl.h @@ -46,7 +46,7 @@ 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 las arm link frame). + Matrix4d tip_in_baselink; // Arm forward kinematics (from base_link to arm link frame). Matrix4d cam_in_baselink; // Camera in base_link frames. }ht; @@ -87,9 +87,9 @@ typedef struct{ 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. - double cog_gain; // Gain of CoG alignement secondary task. + double cog_gain; // Gain of CoG alignment secondary task. MatrixXd cog_vel; // Secondary task velocity. - MatrixXd cog_PGxy; // X and Y fo the CoG r.t. Quadrotor body inertial frame. + 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. @@ -105,7 +105,7 @@ class CQuadarm_Task_Priority_Ctrl private: - ht T_; // Homogenous Transforms + ht T_; // Homogeneous Transforms obj_vel vel_; // Robot velocities @@ -120,7 +120,7 @@ class CQuadarm_Task_Priority_Ctrl /** * \brief Compute Quadrotor and Arm Kinematics * - * Compute the quadrotor and arm jacobian and the position of the quadrotor using forward kinematics + * Compute the quadrotor and arm Jacobian and the position of the quadrotor using forward kinematics * */ void qa_kinematics(); @@ -160,13 +160,13 @@ class CQuadarm_Task_Priority_Ctrl /** * \brief Get weighted generalized Jacobian inverse * - * Compute the jacobian inverse weighted depending on target distance + * Compute the Jacobian inverse weighted depending on target distance * */ void weight_inverse(const MatrixXd& J, MatrixXd& J_inverse); /** - * \brief Secondary task gravity alignement + * \brief Secondary task gravity alignment * * Compute vector of the secondary task to vertically align the arm center of gravity * @@ -184,74 +184,16 @@ class CQuadarm_Task_Priority_Ctrl void sec_task_jl(MatrixXd& JG,MatrixXd& sigmaG); /** - * \brief Compute Desired pose velocities at the jacobian null space (over-determined system) - * - * Compute the vector of motion of the jacobian null space - * - */ - void old_sec_task_vel_unina(); - - /** - * \brief Compute Desired pose velocities at the jacobian null space (over-determined system) - * - * Compute the vector of motion of the jacobian null space - * - */ - void old_sec_task_vel_catec(); - - /** - * \brief Secondary task kinematic stabilization - * - * Compute those solutions that accomplish the primary task and also that reaches horizontal arm base. - * - */ - MatrixXd old_sec_task_stabil(); - - /** - * \brief Get Arm base angles due to arm kinematics - * - * Compute the arm kinematics to know the produced angles in the arm base. - * - */ - MatrixXd old_get_stabil_angles(const MatrixXd &qa); - - /** - * \brief OLD Secondary task gravity alignement - * - * Compute vector of the secondary task to vertically align the arm center of gravity - * - * with the quadrotor gravitational vector - * - */ - MatrixXd old_sec_task_cg(); - - /** - * \brief OLD Secondary task Arm joint limits avoidance - * - * Compute vector of the secondary task to avoid arm joint limits - * - */ - MatrixXd old_sec_task_jointlimits(); - - /** - * \brief Get arm's center of gravity distance - * - * Get arm's center of gravity distance - * - */ - MatrixXd old_get_cg_dist(const MatrixXd &qa,const vector<Segment> &arm_segment); - - /** - * \bried Matrix pseudo-inverse + * \brief Matrix pseudo-inverse * * Compute the matrix pseudo-inverse using SVD */ MatrixXd calcPinv(const MatrixXd &a); /** - * \bried KDL Frame to Homogenous transform + * \brief KDL Frame to Homogeneous transform * - * Compute the convertion from KDL Frame to Homogenous transform (Eigen Matrix 4f) + * Compute the conversion from KDL Frame to Homogeneous transform (Eigen Matrix 4f) */ Matrix4d GetTransform(const Frame &f); @@ -285,7 +227,6 @@ class CQuadarm_Task_Priority_Ctrl ctrl_params& ctrl_params, MatrixXd& joint_pos, MatrixXd& robot_vel); - /** * \brief Write to file *