From e9601a0ecd3f49bf99a2b41421ba6cbf5f0a73f6 Mon Sep 17 00:00:00 2001
From: asantamaria <asantamaria@iri.upc.edu>
Date: Tue, 23 Aug 2016 17:27:36 +0200
Subject: [PATCH] Changes to Skew in CoG Function
---
src/tasks/cog.cpp | 10 +---------
src/tasks/cog.h | 1 +
2 files changed, 2 insertions(+), 9 deletions(-)
diff --git a/src/tasks/cog.cpp b/src/tasks/cog.cpp
index 496394e..03f8bd3 100644
--- a/src/tasks/cog.cpp
+++ b/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);
diff --git a/src/tasks/cog.h b/src/tasks/cog.h
index de41bec..22dcf15 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 {
--
GitLab