From fd6b8c81adce6b33239aef9b761cfa6c92e98137 Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Wed, 12 Dec 2018 16:05:22 -0200 Subject: [PATCH] Fix bug in scope of tree and chain. --- .../computed_torque_controller/computed_torque_controller.h | 4 +++- src/computed_torque_controller.cpp | 10 ++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/include/computed_torque_controller/computed_torque_controller.h b/include/computed_torque_controller/computed_torque_controller.h index d9ffae3..5a8d61a 100644 --- a/include/computed_torque_controller/computed_torque_controller.h +++ b/include/computed_torque_controller/computed_torque_controller.h @@ -49,7 +49,9 @@ namespace effort_controllers int nJoints_; ros::Subscriber sub_command_; - + + KDL::Tree tree_; + KDL::Chain chain_; KDL::ChainIdSolver_RNE *idsolver_; KDL::JntArray q_; diff --git a/src/computed_torque_controller.cpp b/src/computed_torque_controller.cpp index d4445c0..0dc4574 100644 --- a/src/computed_torque_controller.cpp +++ b/src/computed_torque_controller.cpp @@ -74,8 +74,7 @@ namespace effort_controllers return false; } - KDL::Tree tree; - if (!kdl_parser::treeFromString(robot_desc_string,tree)) + if (!kdl_parser::treeFromString(robot_desc_string,tree_)) { ROS_ERROR("Failed to construct KDL tree."); return false; @@ -95,8 +94,7 @@ namespace effort_controllers return false; } - KDL::Chain chain; - if (!tree.getChain(chainRoot,chainTip,chain)) + if (!tree_.getChain(chainRoot,chainTip,chain_)) { ROS_ERROR("Failed to get chain from KDL tree."); return false; @@ -107,7 +105,7 @@ namespace effort_controllers node_.param("gravity/y",g[1],0.0); node_.param("gravity/z",g[2],-9.8); - if((idsolver_=new KDL::ChainIdSolver_RNE(chain,g)) == NULL) + if((idsolver_=new KDL::ChainIdSolver_RNE(chain_,g)) == NULL) { ROS_ERROR("Failed to create ChainIDSolver_RNE."); return false; @@ -121,7 +119,7 @@ namespace effort_controllers ddqr_.resize(nJoints_); torque_.resize(nJoints_); - fext_.resize(chain.getNrOfSegments()); + fext_.resize(chain_.getNrOfSegments()); Kp_.resize(nJoints_,nJoints_); Kd_.resize(nJoints_,nJoints_); -- 2.12.0