diff --git a/src/common_obj.h b/src/common_obj.h
index 65843b3098e886d0db899a349f432a7c48209d83..722021ac5c4fa2c267b659d65f339d8b476659b7 100644
--- a/src/common_obj.h
+++ b/src/common_obj.h
@@ -86,6 +86,7 @@ class CHT{
Eigen::Matrix4d cam_in_baselink; // Camera in base_link frames.
Eigen::Matrix4d tip_in_baselink; // Arm forward kinematics (from base_link to arm link frame).
Eigen::Matrix4d armbase_in_baselink; // Camera in base_link frames.
+ Eigen::Matrix4d link1_in_armbase; // Arm link 1 in arm base frame.
Eigen::Matrix4d baselink_in_w; // base_link in World frames.
Eigen::Matrix4d cam_in_w; // Camera in World frames.
diff --git a/src/tasks/cog.cpp b/src/tasks/cog.cpp
index 74b9dd8e7836052bba39b64f9bd1995f3f0079f7..9283a370d15d6d95f8e9a319d2c279d6321bee9d 100644
--- a/src/tasks/cog.cpp
+++ b/src/tasks/cog.cpp
@@ -119,9 +119,7 @@ void CTaskCOG::CoGCommon(UAM::CArm& arm, const UAM::CHT& HT, Eigen::MatrixXd& co
// std::cout << "cog: " << 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
- // 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 = Eigen::MatrixXd::Identity(3,3);
+ Rib = HT.baselink_in_w.block(0,0,3,3)*HT.armbase_in_baselink.block(0,0,3,3);
// X and Y values of CoG in quadrotor inertial frame
cog_arm = arm_cog_data.arm_cog.block(0,0,3,1);
@@ -143,15 +141,7 @@ void CTaskCOG::CoGCommon(UAM::CArm& arm, const UAM::CHT& HT, Eigen::MatrixXd& co
if(partial_mass!=0){
CoG_partial = partial_arm_cg/partial_mass;
-
- Eigen::Matrix3d screw_rot = Eigen::Matrix3d::Zero();
- screw_rot(0,1) = -IIIrdcolRot_b_x.at(jj)(2);
- screw_rot(0,2) = IIIrdcolRot_b_x.at(jj)(1);
- screw_rot(1,0) = IIIrdcolRot_b_x.at(jj)(2);
- screw_rot(0,2) = -IIIrdcolRot_b_x.at(jj)(0);
- screw_rot(2,0) = -IIIrdcolRot_b_x.at(jj)(1);
- screw_rot(2,1) = IIIrdcolRot_b_x.at(jj)(0);
-
+ Eigen::Matrix3d screw_rot = CCommon_Fc::Skew(IIIrdcolRot_b_x.at(jj));
Jj_cog.col(jj) = (partial_mass/arm.mass)*screw_rot*CoG_partial.block(0,0,3,1);
}
else Jj_cog.col(jj) = Eigen::MatrixXd::Zero(3,1);
@@ -161,19 +151,26 @@ void CTaskCOG::CoGCommon(UAM::CArm& arm, const UAM::CHT& HT, Eigen::MatrixXd& co
void CTaskCOG::TaskErrorJacFull(UAM::CArm& arm, const UAM::CHT& HT, Eigen::MatrixXd& cog_PGxy, Eigen::MatrixXd& cog_arm, Eigen::MatrixXd& sigmaG, Eigen::MatrixXd& JG, Eigen::MatrixXd& JG_pseudo, UAM::ArmCogData& arm_cog_data)
{
Eigen::Matrix3d Rib;
- Eigen::MatrixXd Jj_cog(3,arm.nj);
+ Eigen::MatrixXd Jj_cog(3,arm.nj);
CoGCommon(arm, HT, cog_PGxy, cog_arm, Rib, Jj_cog, arm_cog_data);
+ // Task Error
+ sigmaG = -cog_PGxy.array();
+ // Task Jacobian
+ JG = Eigen::MatrixXd::Zero(2,4+arm.nj);
+ Eigen::Matrix3d Rb_in_w = HT.baselink_in_w.block(0,0,3,3);
+ Eigen::Matrix3d Rarmb_in_b = HT.armbase_in_baselink.block(0,0,3,3);
+ Eigen::Matrix3d Rl1_in_armb = HT.link1_in_armbase.block(0,0,3,3);
+ Eigen::Matrix3d skew_rot = UAM::CCommon_Fc::Skew(Rb_in_w*Rl1_in_armb*arm_cog_data.arm_cog.block(0,0,3,1));
+ JG.block(0,3,2,1) = skew_rot.block(0,2,2,1);
Eigen::MatrixXd Jacob_temp(3,arm.nj);
for (unsigned int ii = 0; ii < arm.nj; ++ii)
- Jacob_temp.col(ii) = Rib * Jj_cog.col(ii);
+ Jacob_temp.col(ii) = Rb_in_w * Rarmb_in_b * Rl1_in_armb * Jj_cog.col(ii);
+
+ JG.block(0,4,2,arm.nj)=Jacob_temp.block(0,0,2,arm.nj);
- JG = Eigen::MatrixXd::Zero(2,4+arm.nj);
- JG.block(0,4,2,arm.nj) = Jacob_temp.block(0,0,2,arm.nj);
//JG_pseudo = UAM::CCommon_Fc::CalcPinv(JG);
JG_pseudo = JG.transpose();
-
- sigmaG = -cog_PGxy;
}
void CTaskCOG::TaskErrorJac(UAM::CArm& arm, const UAM::CHT& HT, Eigen::MatrixXd& cog_PGxy, Eigen::MatrixXd& cog_arm, Eigen::MatrixXd& sigmaG, Eigen::MatrixXd& JG, Eigen::MatrixXd& JG_pseudo, UAM::ArmCogData& arm_cog_data)
@@ -181,19 +178,22 @@ void CTaskCOG::TaskErrorJac(UAM::CArm& arm, const UAM::CHT& HT, Eigen::MatrixXd&
Eigen::Matrix3d Rib;
Eigen::MatrixXd Jj_cog(3,arm.nj);
CoGCommon(arm, HT, cog_PGxy, cog_arm, Rib, Jj_cog, arm_cog_data);
+ // Task Error
+ sigmaG = -(cog_PGxy.transpose()*cog_PGxy);
+ // Task Jacobian
JG = Eigen::MatrixXd::Zero(1,4+arm.nj);
- JG.block(0,0,1,4) = Eigen::MatrixXd::Zero(1,4);
+ Eigen::Matrix3d Rb_in_w = HT.baselink_in_w.block(0,0,3,3);
+ Eigen::Matrix3d Rarmb_in_b = HT.armbase_in_baselink.block(0,0,3,3);
+ Eigen::Matrix3d Rl1_in_armb = HT.link1_in_armbase.block(0,0,3,3);
Eigen::MatrixXd Jacob_temp(3,arm.nj);
for (unsigned int ii = 0; ii < arm.nj; ++ii)
- Jacob_temp.col(ii) = Rib * Jj_cog.col(ii);
+ Jacob_temp.col(ii) = Rb_in_w * Rarmb_in_b * Rl1_in_armb * Jj_cog.col(ii);
JG.block(0,4,1,arm.nj)=2*cog_PGxy.transpose()*Jacob_temp.block(0,0,2,arm.nj);
-
+
//JG_pseudo = UAM::CCommon_Fc::CalcPinv(JG);
- JG_pseudo = JG.transpose();
-
- sigmaG = -(cog_PGxy.transpose()*cog_PGxy);
+ JG_pseudo = JG.transpose();
}
} // End of UAM namespace
\ No newline at end of file
diff --git a/src/tasks/cog.h b/src/tasks/cog.h
index de41becce7f22971564087695fa8b3f4acb71ea7..d3c5afe1601b74dece50fd5c0af70bdf0389315f 100644
--- a/src/tasks/cog.h
+++ b/src/tasks/cog.h
@@ -6,6 +6,7 @@
// Own
#include <common_obj.h>
+#include <common_fc.h>
namespace UAM {
@@ -55,7 +56,6 @@ class CTaskCOG
*
*/
static void TaskErrorJacFull(UAM::CArm& arm, const UAM::CHT& HT, Eigen::MatrixXd& cog_PGxy, Eigen::MatrixXd& cog_arm, Eigen::MatrixXd& sigmaG, Eigen::MatrixXd& JG, Eigen::MatrixXd& JG_pseudo, UAM::ArmCogData& arm_cog_data);
-
};
} // End of UAM namespace
diff --git a/src/tasks/eepos.cpp b/src/tasks/eepos.cpp
index 9433c9e78087845f3073b7b2a74001ad643f3d79..29c9d90f609ae0670857a381c3780b97e2654d29 100644
--- a/src/tasks/eepos.cpp
+++ b/src/tasks/eepos.cpp
@@ -149,9 +149,10 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M
Eigen::MatrixXd ArmJac = UAM::CKinematics::ArmJac(arm);
Eigen::MatrixXd RRb_in_w = Eigen::MatrixXd::Zero(6,6);
- Eigen::Matrix3d Rarmb_in_b = Eigen::Vector3d(-1, 1,-1).asDiagonal();
- RRb_in_w.block(0,0,3,3) = Rb_in_w*Rarmb_in_b;
- RRb_in_w.block(3,3,3,3) = Rb_in_w*Rarmb_in_b;
+ Eigen::Matrix3d Rarmb_in_b = HT.armbase_in_baselink.block(0,0,3,3);
+ Eigen::Matrix3d Rl1_in_armb = HT.link1_in_armbase.block(0,0,3,3);
+ RRb_in_w.block(0,0,3,3) = Rb_in_w * Rarmb_in_b * Rl1_in_armb;
+ RRb_in_w.block(3,3,3,3) = RRb_in_w.block(0,0,3,3);
JEEPOS_omw.block(0,6,6,arm.nj) = RRb_in_w * ArmJac;
Eigen::MatrixXd omega2euldiff_in_w = Eigen::MatrixXd::Zero(6,6);