From d7bbd1f37d0c44bd2736a5dbfe39b6183b38f887 Mon Sep 17 00:00:00 2001
From: dmronga <dennis.mronga@dfki.de>
Date: Tue, 21 Nov 2023 16:49:23 +0100
Subject: [PATCH] Add joint blacklist property

---
 src/core/RobotModelConfig.hpp                      | 2 ++
 src/robot_models/hyrodyn/RobotModelHyrodyn.cpp     | 1 +
 src/robot_models/kdl/RobotModelKDL.cpp             | 7 ++++---
 src/robot_models/pinocchio/RobotModelPinocchio.cpp | 3 ++-
 src/robot_models/rbdl/RobotModelRBDL.cpp           | 2 +-
 5 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/src/core/RobotModelConfig.hpp b/src/core/RobotModelConfig.hpp
index 2a83eaa4..72ddb74d 100644
--- a/src/core/RobotModelConfig.hpp
+++ b/src/core/RobotModelConfig.hpp
@@ -67,6 +67,8 @@ struct RobotModelConfig{
     bool floating_base;
     /** Optional: Link names that are possibly in contact with the environment. These have to be valid link names in the robot model.*/
     ActiveContacts contact_points;
+
+    std::vector<std::string> joint_blacklist;
 };
 
 }
diff --git a/src/robot_models/hyrodyn/RobotModelHyrodyn.cpp b/src/robot_models/hyrodyn/RobotModelHyrodyn.cpp
index 21e7bdc5..7ed68466 100644
--- a/src/robot_models/hyrodyn/RobotModelHyrodyn.cpp
+++ b/src/robot_models/hyrodyn/RobotModelHyrodyn.cpp
@@ -33,6 +33,7 @@ bool RobotModelHyrodyn::configure(const RobotModelConfig& cfg){
         return false;
     }
     base_frame = robot_urdf->getRoot()->name;
+    URDFTools::applyJointBlacklist(robot_urdf, cfg.joint_blacklist);
 
     // Add floating base
     has_floating_base = cfg.floating_base;
diff --git a/src/robot_models/kdl/RobotModelKDL.cpp b/src/robot_models/kdl/RobotModelKDL.cpp
index 83f66fef..5e544724 100644
--- a/src/robot_models/kdl/RobotModelKDL.cpp
+++ b/src/robot_models/kdl/RobotModelKDL.cpp
@@ -38,6 +38,7 @@ bool RobotModelKDL::configure(const RobotModelConfig& cfg){
         LOG_ERROR("Unable to parse urdf model")
         return false;
     }
+    URDFTools::applyJointBlacklist(robot_urdf, cfg.joint_blacklist);
     base_frame =  robot_urdf->getRoot()->name;
 
     // Joint names from URDF without floating base
@@ -313,7 +314,7 @@ const base::MatrixXd &RobotModelKDL::comJacobian(){
         if(segmentName == full_tree.getRootSegment()->second.segment.getName())
             continue;
         std::string segmentNameCOG = segmentName + "_COG";
-        
+
         KDL::Vector segmentCOG = segment.getInertia().getCOG();
         double segmentMass = segment.getInertia().getMass();
 
@@ -328,7 +329,7 @@ const base::MatrixXd &RobotModelKDL::comJacobian(){
 
         COGSegmentNames.push_back(segmentNameCOG);
     }
-    
+
     // compute com jacobian as (mass) weighted average over COG frame jacobians
     for(const auto& segment_name : COGSegmentNames)
         com_jac += (mass_map[segment_name] / totalMass) *
@@ -427,7 +428,7 @@ void RobotModelKDL::recursiveCOM(const KDL::SegmentMap::const_iterator& currentS
     KDL::Segment segment = currentSegment->second.segment;
 #endif
     // If the segment has a real joint, get the joint position
-    if( segment.getJoint().getType() != KDL::Joint::None ) {        
+    if( segment.getJoint().getType() != KDL::Joint::None ) {
         const std::string name = segment.getJoint().getName();
         if(joint_idx_map_kdl.count(name)==0){
             LOG_ERROR_S  << "recursiveCOM: KDL tree contains joint " << name << " but this joint does not exist in joint idx map" << std::endl;
diff --git a/src/robot_models/pinocchio/RobotModelPinocchio.cpp b/src/robot_models/pinocchio/RobotModelPinocchio.cpp
index 948751bf..296ae67c 100644
--- a/src/robot_models/pinocchio/RobotModelPinocchio.cpp
+++ b/src/robot_models/pinocchio/RobotModelPinocchio.cpp
@@ -40,6 +40,7 @@ bool RobotModelPinocchio::configure(const RobotModelConfig& cfg){
         return false;
     }
     base_frame =  robot_urdf->getRoot()->name;
+    URDFTools::applyJointBlacklist(robot_urdf, cfg.joint_blacklist);
 
     try{
         if(cfg.floating_base){
@@ -362,7 +363,7 @@ const base::MatrixXd &RobotModelPinocchio::jacobianDot(const std::string &root_f
     throw std::runtime_error("Not implemented: jacobianDot has not been implemented for RobotModelPinocchio");
 }
 
-const base::MatrixXd &RobotModelPinocchio::jointSpaceInertiaMatrix(){   
+const base::MatrixXd &RobotModelPinocchio::jointSpaceInertiaMatrix(){
 
     if(joint_state.time.isNull()){
         LOG_ERROR("RobotModelPinocchio: You have to call update() with appropriately timestamped joint data at least once before requesting kinematic information!");
diff --git a/src/robot_models/rbdl/RobotModelRBDL.cpp b/src/robot_models/rbdl/RobotModelRBDL.cpp
index 4f1014aa..9cc4fe7d 100644
--- a/src/robot_models/rbdl/RobotModelRBDL.cpp
+++ b/src/robot_models/rbdl/RobotModelRBDL.cpp
@@ -40,7 +40,7 @@ bool RobotModelRBDL::configure(const RobotModelConfig& cfg){
         return false;
     }
     base_frame = robot_urdf->getRoot()->name;
-
+    URDFTools::applyJointBlacklist(robot_urdf, cfg.joint_blacklist);
     joint_names = URDFTools::jointNamesFromURDF(robot_urdf);
 
     // Temporary workaround: RBDL does not support rotations of the link invertias. Set them to zero.