Add joint blacklist property

ARC-OPT · Nov 21, 2023 · d7bbd1f · d7bbd1f
1 parent 97aa2e8
commit d7bbd1f
Show file tree

Hide file tree

Showing 5 changed files with 10 additions and 5 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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.