Compatibility with 3dof forces

ADVRHumanoids · Apr 17, 2024 · 6f4cccf · 6f4cccf
1 parent bbafbe6
commit 6f4cccf
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 3 deletions.
diff --git a/src/opensot/OpenSotImpl.cpp b/src/opensot/OpenSotImpl.cpp
@@ -507,10 +507,10 @@ bool OpenSotImpl::update(double time, double period)
 
         _model->getJacobian(link_name, _J);
 
-        Eigen::Vector6d f_value;
+        Eigen::VectorXd f_value;
         p.second.getValue(_x, f_value);
 
-        _tau.noalias() -= _J.transpose() * f_value;
+        _tau.noalias() -= _J.block(0, 0, f_value.size(), _J.cols()).transpose() * f_value;
 
         if(_logger)
         {

diff --git a/src/ros/RosServerClass.cpp b/src/ros/RosServerClass.cpp
@@ -190,7 +190,13 @@ void XBot::Cartesian::RosServerClass::publish_solution(ros::Time time)
         auto frame = p.first.substr(6); // removes "force_"
 
         Eigen::Affine3d w_T_f = _model->getPose(frame);
-        Eigen::Vector6d ww = p.second;
+        Eigen::Vector6d ww;
+        ww.setZero();
+        if(p.second.size() == 3)
+            ww.segment(0,3) = p.second;
+        else
+            ww = p.second;
+
         //in local frame
         ww.segment(0,3) = w_T_f.linear().inverse() * ww.segment(0,3);
         ww.segment(3,3) = w_T_f.linear().inverse() * ww.segment(3,3);