diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index 04c21592e63444fcb05d8eec0ec5bdceba8c2fc8..d2c8e720b695606f2999a0840c3b137a03c0ba8a 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -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,
const goal_obj& goal,
ht& HT,
- const arm& arm,
+ arm& arm,
const quadrotor& quad,
ctrl_params& ctrl_params,
MatrixXd& robot_pos,
@@ -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);
// 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_;
robot_pos.block(0,0,6,1) = this->quad_.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
void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,MatrixXd& sigmaG)
{
// Initializations
- MatrixXd qa; // joint positions
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
@@ -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)
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
MatrixXd pini(4,1);
pini.col(0) << 0,0,0,1;
// 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
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;
// Debug
- //std::cout << "**************" << std::endl;
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)
{
// Get mass of each link
mass(jj,0) = arm_segment.at(jj+1).getInertia().getMass();
// 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)
Transf.at(jj) = GetTransform(joint_pose.at(jj));
+
+//TODO: Debugging till here
+
// 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
@@ -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 << "Transform: " << 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
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);
+ // this->arm_.T_base_to_joint.at(jj+1) = Transf.at(jj);
}
+
// Debug
// std::cout << "mass: " << this->arm_.mass << std::endl;
// std::cout << "arm_cg: " << arm_cg << std::endl;
@@ -654,16 +658,19 @@ Matrix4d CQuadarm_Task_Priority_Ctrl::GetTransform(const Frame &f)
Matrix3d R;
Matrix4d T;
- f.M.GetEulerZYX(yaw,pitch,roll);
+ // f.M.GetEulerZYX(yaw,pitch,roll);
+ f.M.GetRPY(roll,pitch,yaw);
// euler convention zyx
- R = Eigen::AngleAxisd(yaw, Vector3d::UnitZ()) \
+ R = Eigen::AngleAxisd(roll, Vector3d::UnitX()) \
* Eigen::AngleAxisd(pitch, Vector3d::UnitY()) \
- * Eigen::AngleAxisd(roll, Vector3d::UnitX());
+ * Eigen::AngleAxisd(yaw, Vector3d::UnitZ());
T.block(0,0,3,3)=R;
- T(0,3) = (double)f.p.data[0];
- T(1,3) = (double)f.p.data[1];
- T(2,3) = (double)f.p.data[2];
+ // Vector3d kdltrans((double)f.p.x(),(double)f.p.y(),(double)f.p.z());
+ // T.block(0,3,3,1) = R.transpose()*kdltrans;
+ 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;
return T;
diff --git a/src/quadarm_task_priority_ctrl.h b/src/quadarm_task_priority_ctrl.h
index 1417d1e9f355cb7a28294d658085d7b1dcd9f8c6..91f797edb16660f8d1afa822a04bf3177e676e19 100644
--- a/src/quadarm_task_priority_ctrl.h
+++ b/src/quadarm_task_priority_ctrl.h
@@ -48,6 +48,7 @@ typedef struct{
Matrix4d cam_in_tip; // Camera in arm tip frames.
Matrix4d tip_in_baselink; // Arm forward kinematics (from base_link to arm link frame).
Matrix4d cam_in_baselink; // Camera in base_link frames.
+ Matrix4d armbase_in_baselink; // Camera in base_link frames.
}ht;
// Arm joint definition
@@ -69,6 +70,7 @@ typedef struct{
boost::scoped_ptr<ChainJntToJacSolver> jnt_to_jac_solver; // chain solver.
JntArray jnt_pos_kdl; // Array of arm positions.
Jacobian jacobian; // Jacobian.
+ vector<MatrixXd> T_base_to_joint; // Homogenous Transforms from arm base to each link.
}arm;
// Quadrotor definition
@@ -265,7 +267,7 @@ class CQuadarm_Task_Priority_Ctrl
void quadarm_task_priority_ctrl(const double& quad_height,
const goal_obj& goal,
ht& HT,
- const arm& arm,
+ arm& arm,
const quadrotor& quad,
ctrl_params& ctrl_params,
MatrixXd& robot_pos,