Changes to Skew in CoG Function

src/tasks/cog.cpp

@@ -143,15 +143,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);

src/tasks/cog.h

@@ -6,6 +6,7 @@
 
 // Own
 #include <common_obj.h>
+#include <common_fc.h>
 
 namespace UAM {