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

debugging. till line 435

parent ac1da1ca
No related branches found
No related tags found
No related merge requests found
...@@ -17,7 +17,7 @@ CQuadarm_Task_Priority_Ctrl::~CQuadarm_Task_Priority_Ctrl() ...@@ -17,7 +17,7 @@ CQuadarm_Task_Priority_Ctrl::~CQuadarm_Task_Priority_Ctrl()
void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const double& quad_height, void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const double& quad_height,
const goal_obj& goal, const goal_obj& goal,
ht& HT, ht& HT,
const arm& arm, arm& arm,
const quadrotor& quad, const quadrotor& quad,
ctrl_params& ctrl_params, ctrl_params& ctrl_params,
MatrixXd& robot_pos, MatrixXd& robot_pos,
...@@ -52,7 +52,8 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const double& quad_ ...@@ -52,7 +52,8 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const double& quad_
this->quad_.pos = this->quad_.pos+(this->uam_.vel.block(0,0,6,1) * this->ctrl_params_.dt); this->quad_.pos = this->quad_.pos+(this->uam_.vel.block(0,0,6,1) * this->ctrl_params_.dt);
// return values // return values
HT = this->T_; HT = this->T_; // DEBUG. TODO: make it constant and do not use this trans. outside.
arm.T_base_to_joint = this->arm_.T_base_to_joint; // DEBUG. TODO: make it constant and do not use this trans. outside.
ctrl_params = this->ctrl_params_; ctrl_params = this->ctrl_params_;
robot_pos.block(0,0,6,1) = this->quad_.pos; robot_pos.block(0,0,6,1) = this->quad_.pos;
robot_pos.block(6,0,this->arm_.nj,1) = this->arm_.jnt_pos; robot_pos.block(6,0,this->arm_.nj,1) = this->arm_.jnt_pos;
...@@ -375,7 +376,6 @@ void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,Mat ...@@ -375,7 +376,6 @@ 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) void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,MatrixXd& sigmaG)
{ {
// Initializations // Initializations
MatrixXd qa; // joint positions
this->arm_.mass = 0; // Total arm mass this->arm_.mass = 0; // Total arm mass
MatrixXd mass = MatrixXd::Zero(this->arm_.nj,1); // Mass of the links MatrixXd mass = MatrixXd::Zero(this->arm_.nj,1); // Mass of the links
this->ctrl_params_.cog_arm = MatrixXd::Zero(3,1); // Arm CoG this->ctrl_params_.cog_arm = MatrixXd::Zero(3,1); // Arm CoG
...@@ -403,40 +403,37 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr ...@@ -403,40 +403,37 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
for (unsigned int ii = 1; ii < js; ++ii) for (unsigned int ii = 1; ii < js; ++ii)
arm_segment.at(ii) = this->arm_.chain.getSegment(ii); arm_segment.at(ii) = this->arm_.chain.getSegment(ii);
// Get joint positions
qa = this->arm_.jnt_pos;
//Fixed transform from arm_base to base_link
// Matrix4d Tbase = Matrix4d::Zero();
// Tbase(0,0) = -1;
// Tbase(1,1) = 1;
// Tbase(2,2) = -1;
// Tbase(3,3) = 1;
Matrix4d Tbase = Matrix4d::Identity();
// Origin of the base frame // Origin of the base frame
MatrixXd pini(4,1); MatrixXd pini(4,1);
pini.col(0) << 0,0,0,1; pini.col(0) << 0,0,0,1;
// Initial transform // Initial transform
T_base_to_joint.at(0) = Tbase; // T_base_to_joint.at(0) = this->T_.armbase_in_baselink;
T_base_to_joint.at(0) = Matrix4d::Identity();
// 3rd column of rotation matrix // 3rd column of rotation matrix
IIIrdcolRot_b_x.at(0) = T_base_to_joint.at(0).block(0,2,3,1); IIIrdcolRot_b_x.at(0) = T_base_to_joint.at(0).block(0,2,3,1);
// Origin of the initial frame // Origin of the initial frame
frame_link.at(0) = T_base_to_joint.at(0) * pini; frame_link.at(0) = T_base_to_joint.at(0) * pini;
// Debug // Debug
//std::cout << "**************" << std::endl;
this->arm_cog_data_.link.resize(this->arm_.nj); 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);
std::cout << "**************" << std::endl;
for (unsigned int jj = 0; jj < this->arm_.nj; ++jj) for (unsigned int jj = 0; jj < this->arm_.nj; ++jj)
{ {
// Get mass of each link // Get mass of each link
mass(jj,0) = arm_segment.at(jj+1).getInertia().getMass(); mass(jj,0) = arm_segment.at(jj+1).getInertia().getMass();
// Joint frames // Joint frames
joint_pose.at(jj) = arm_segment.at(jj+1).pose(qa(jj,0)); joint_pose.at(jj) = arm_segment.at(jj+1).pose(this->arm_.jnt_pos(jj,0));
// relative Homogeneous transforms (HT) // relative Homogeneous transforms (HT)
Transf.at(jj) = GetTransform(joint_pose.at(jj)); Transf.at(jj) = GetTransform(joint_pose.at(jj));
//TODO: Debugging till here
// HT r.t. base link // HT r.t. base link
T_base_to_joint.at(jj+1) = T_base_to_joint.at(jj) * Transf.at(jj); T_base_to_joint.at(jj+1) = T_base_to_joint.at(jj) * Transf.at(jj);
// 3rd column of rotation matrix // 3rd column of rotation matrix
...@@ -461,12 +458,19 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr ...@@ -461,12 +458,19 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
// std::cout << frame_link.at(jj)+dist.at(jj) << std::endl; // std::cout << frame_link.at(jj)+dist.at(jj) << std::endl;
// std::cout << "Transform: " << std::endl; // std::cout << "Transform: " << std::endl;
// std::cout << T_base_to_joint.at(jj+1) <<std::endl; // std::cout << T_base_to_joint.at(jj+1) <<std::endl;
std::cout << "______joint: " << jj << std::endl;
std::cout << Transf.at(jj).block(0,3,3,1) << std::endl;
Matrix3d Rot = Transf.at(jj).block(0,0,3,3);
std::cout << Rot.eulerAngles(0, 1, 2) << std::endl;
// Fill link information // 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).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).cog = (frame_link.at(jj+1)-frame_link.at(jj))/2;
this->arm_cog_data_.link.at(jj).mass = mass(jj,0); 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);
// this->arm_.T_base_to_joint.at(jj+1) = Transf.at(jj);
} }
// Debug // Debug
// std::cout << "mass: " << this->arm_.mass << std::endl; // std::cout << "mass: " << this->arm_.mass << std::endl;
// std::cout << "arm_cg: " << arm_cg << std::endl; // std::cout << "arm_cg: " << arm_cg << std::endl;
...@@ -654,16 +658,19 @@ Matrix4d CQuadarm_Task_Priority_Ctrl::GetTransform(const Frame &f) ...@@ -654,16 +658,19 @@ Matrix4d CQuadarm_Task_Priority_Ctrl::GetTransform(const Frame &f)
Matrix3d R; Matrix3d R;
Matrix4d T; Matrix4d T;
f.M.GetEulerZYX(yaw,pitch,roll); // f.M.GetEulerZYX(yaw,pitch,roll);
f.M.GetRPY(roll,pitch,yaw);
// euler convention zyx // euler convention zyx
R = Eigen::AngleAxisd(yaw, Vector3d::UnitZ()) \ R = Eigen::AngleAxisd(roll, Vector3d::UnitX()) \
* Eigen::AngleAxisd(pitch, Vector3d::UnitY()) \ * Eigen::AngleAxisd(pitch, Vector3d::UnitY()) \
* Eigen::AngleAxisd(roll, Vector3d::UnitX()); * Eigen::AngleAxisd(yaw, Vector3d::UnitZ());
T.block(0,0,3,3)=R; T.block(0,0,3,3)=R;
T(0,3) = (double)f.p.data[0]; // Vector3d kdltrans((double)f.p.x(),(double)f.p.y(),(double)f.p.z());
T(1,3) = (double)f.p.data[1]; // T.block(0,3,3,1) = R.transpose()*kdltrans;
T(2,3) = (double)f.p.data[2]; T(0,3) = (double)f.p.x();
T(1,3) = (double)f.p.y();
T(2,3) = (double)f.p.z();
T.row(3) << 0, 0, 0, 1; T.row(3) << 0, 0, 0, 1;
return T; return T;
......
...@@ -48,6 +48,7 @@ typedef struct{ ...@@ -48,6 +48,7 @@ typedef struct{
Matrix4d cam_in_tip; // Camera in arm tip frames. Matrix4d cam_in_tip; // Camera in arm tip frames.
Matrix4d tip_in_baselink; // Arm forward kinematics (from base_link to 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. Matrix4d cam_in_baselink; // Camera in base_link frames.
Matrix4d armbase_in_baselink; // Camera in base_link frames.
}ht; }ht;
// Arm joint definition // Arm joint definition
...@@ -69,6 +70,7 @@ typedef struct{ ...@@ -69,6 +70,7 @@ typedef struct{
boost::scoped_ptr<ChainJntToJacSolver> jnt_to_jac_solver; // chain solver. boost::scoped_ptr<ChainJntToJacSolver> jnt_to_jac_solver; // chain solver.
JntArray jnt_pos_kdl; // Array of arm positions. JntArray jnt_pos_kdl; // Array of arm positions.
Jacobian jacobian; // Jacobian. Jacobian jacobian; // Jacobian.
vector<MatrixXd> T_base_to_joint; // Homogenous Transforms from arm base to each link.
}arm; }arm;
// Quadrotor definition // Quadrotor definition
...@@ -265,7 +267,7 @@ class CQuadarm_Task_Priority_Ctrl ...@@ -265,7 +267,7 @@ class CQuadarm_Task_Priority_Ctrl
void quadarm_task_priority_ctrl(const double& quad_height, void quadarm_task_priority_ctrl(const double& quad_height,
const goal_obj& goal, const goal_obj& goal,
ht& HT, ht& HT,
const arm& arm, arm& arm,
const quadrotor& quad, const quadrotor& quad,
ctrl_params& ctrl_params, ctrl_params& ctrl_params,
MatrixXd& robot_pos, MatrixXd& robot_pos,
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment