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

debugged all tasks, however sub tasks not working...

parent 91f3f099
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/quadarm_task_priority_ctrl.cpp
+ 45
− 74
View file @ 2e141bb0
......@@ -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);
......
src/quadarm_task_priority_ctrl.h
+ 2
− 8
View file @ 2e141bb0
......@@ -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;
......
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