From 992bac65483acbbc93bf80f692300c0d5f68aac6 Mon Sep 17 00:00:00 2001 From: fmessmer Date: Sat, 20 Nov 2021 17:46:24 +0100 Subject: [PATCH 1/8] extend debug_evaluate_jointstates_node to recursively publish cart vel magnitudes for frames of chain --- .../debug/debug_evaluate_jointstates_node.cpp | 43 ++++++++++++++++--- 1 file changed, 37 insertions(+), 6 deletions(-) diff --git a/cob_twist_controller/src/debug/debug_evaluate_jointstates_node.cpp b/cob_twist_controller/src/debug/debug_evaluate_jointstates_node.cpp index 60d73330a..765b757ff 100644 --- a/cob_twist_controller/src/debug/debug_evaluate_jointstates_node.cpp +++ b/cob_twist_controller/src/debug/debug_evaluate_jointstates_node.cpp @@ -16,6 +16,8 @@ #include +#include +#include #include #include @@ -38,6 +40,7 @@ class DebugEvaluateJointStates ros::Subscriber jointstate_sub_; ros::Publisher manipulability_pub_; ros::Publisher twist_current_pub_; + ros::Publisher twist_magnitude_pub_; std::string chain_base_link_; std::string chain_tip_link_; @@ -83,6 +86,7 @@ class DebugEvaluateJointStates jointstate_sub_ = nh_.subscribe("joint_states", 1, &DebugEvaluateJointStates::jointstateCallback, this); manipulability_pub_ = nh_.advertise ("debug/manipulability", 1); twist_current_pub_ = nh_.advertise ("debug/twist_current", 1); + twist_magnitude_pub_ = nh_.advertise ("debug/twist_magnitude", 1); return 0; } @@ -92,20 +96,47 @@ class DebugEvaluateJointStates KDL::JntArray q = KDL::JntArray(chain_.getNrOfJoints()); KDL::JntArray q_dot = KDL::JntArray(chain_.getNrOfJoints()); - for (unsigned int i = 0; i < msg->name.size(); i++) + // //TODO: sort wrt kinematic order + // for (unsigned int i = 0; i < msg->name.size(); i++) + // { + // q(i) = msg->position[i]; + // q_dot(i) = msg->velocity[i]; + // } + for (unsigned int i = 0; i < chain_.getNrOfJoints(); i++) { - q(i) = msg->position[i]; - q_dot(i) = msg->velocity[i]; + std::string joint_name = chain_.getSegment(i).getJoint().getName(); + if (std::find(msg->name.begin(), msg->name.end(), joint_name) != msg->name.end()) + { + unsigned int index = std::distance(msg->name.begin(), std::find(msg->name.begin(), msg->name.end(), joint_name)); + q(index) = msg->position[index]; + q_dot(index) = msg->velocity[index]; + } + else + { + ROS_ERROR_STREAM("Joint '" << joint_name << "' not found in JointStates"); + return; + } } /// compute current twist - KDL::FrameVel FrameVel; + std::vector< KDL::FrameVel > v_FrameVel; KDL::JntArrayVel jntArrayVel = KDL::JntArrayVel(q, q_dot); - if (p_fksolver_vel_->JntToCart(jntArrayVel, FrameVel, -1) >= 0) + v_FrameVel.resize(chain_.getNrOfJoints()); + if (p_fksolver_vel_->JntToCart(jntArrayVel, v_FrameVel, chain_.getNrOfJoints()) >= 0) { + //last entry is twist of tip_link geometry_msgs::Twist twist_msg; - tf::twistKDLToMsg(FrameVel.GetTwist(), twist_msg); + tf::twistKDLToMsg(v_FrameVel.back().GetTwist(), twist_msg); twist_current_pub_.publish(twist_msg); + + //recursively calculate FrameVel magnitudes + sensor_msgs::JointState magnitude_msg; + for (unsigned int i = 0; i < chain_.getNrOfJoints(); i++) + { + magnitude_msg.name.push_back(chain_.getSegment(i).getName()); + magnitude_msg.velocity.push_back(v_FrameVel[i].GetTwist().vel.Norm()); + } + twist_magnitude_pub_.publish(magnitude_msg); } /// compute manipulability From 0b6288b4443ef1bb743bdf80b982346faa83e9e5 Mon Sep 17 00:00:00 2001 From: fmessmer Date: Sun, 21 Nov 2021 13:03:09 +0100 Subject: [PATCH 2/8] fill header.stamp in twist_magnitude msg --- .../src/debug/debug_evaluate_jointstates_node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/cob_twist_controller/src/debug/debug_evaluate_jointstates_node.cpp b/cob_twist_controller/src/debug/debug_evaluate_jointstates_node.cpp index 765b757ff..6c40aac50 100644 --- a/cob_twist_controller/src/debug/debug_evaluate_jointstates_node.cpp +++ b/cob_twist_controller/src/debug/debug_evaluate_jointstates_node.cpp @@ -131,6 +131,7 @@ class DebugEvaluateJointStates //recursively calculate FrameVel magnitudes sensor_msgs::JointState magnitude_msg; + magnitude_msg.header.stamp = msg->header.stamp; for (unsigned int i = 0; i < chain_.getNrOfJoints(); i++) { magnitude_msg.name.push_back(chain_.getSegment(i).getName()); From 1a36d0e2fef90484952bd36983750fb42fb66856 Mon Sep 17 00:00:00 2001 From: fmessmer Date: Sun, 21 Nov 2021 13:12:31 +0100 Subject: [PATCH 3/8] provide launch file for debug_evaluate_jointstates --- .../launch/debug_evaluate_jointstates.launch | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 cob_twist_controller/launch/debug_evaluate_jointstates.launch diff --git a/cob_twist_controller/launch/debug_evaluate_jointstates.launch b/cob_twist_controller/launch/debug_evaluate_jointstates.launch new file mode 100644 index 000000000..4c887a246 --- /dev/null +++ b/cob_twist_controller/launch/debug_evaluate_jointstates.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + From 02a5a7b54215b9546cf9c37228db7ef731703ed7 Mon Sep 17 00:00:00 2001 From: fmessmer Date: Tue, 23 Nov 2021 16:02:39 +0100 Subject: [PATCH 4/8] debug chain --- .../src/debug/debug_evaluate_jointstates_node.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/cob_twist_controller/src/debug/debug_evaluate_jointstates_node.cpp b/cob_twist_controller/src/debug/debug_evaluate_jointstates_node.cpp index 6c40aac50..0b3487c95 100644 --- a/cob_twist_controller/src/debug/debug_evaluate_jointstates_node.cpp +++ b/cob_twist_controller/src/debug/debug_evaluate_jointstates_node.cpp @@ -79,6 +79,20 @@ class DebugEvaluateJointStates return -4; } + //debug chain + ROS_WARN("~~~~~~~~~~~~~ DEBUG CHAIN ~~~~~~~~~~~~~"); + ROS_WARN_STREAM("NrOfJoints: "< Date: Tue, 23 Nov 2021 16:03:16 +0100 Subject: [PATCH 5/8] fix getNrOfJoints vs getNrOfSegments and fixed joint handling --- .../debug/debug_evaluate_jointstates_node.cpp | 33 ++++++++++++------- 1 file changed, 21 insertions(+), 12 deletions(-) diff --git a/cob_twist_controller/src/debug/debug_evaluate_jointstates_node.cpp b/cob_twist_controller/src/debug/debug_evaluate_jointstates_node.cpp index 0b3487c95..03ccce839 100644 --- a/cob_twist_controller/src/debug/debug_evaluate_jointstates_node.cpp +++ b/cob_twist_controller/src/debug/debug_evaluate_jointstates_node.cpp @@ -116,27 +116,36 @@ class DebugEvaluateJointStates // q(i) = msg->position[i]; // q_dot(i) = msg->velocity[i]; // } - for (unsigned int i = 0; i < chain_.getNrOfJoints(); i++) + for (unsigned int i = 0; i < chain_.getNrOfSegments(); i++) { - std::string joint_name = chain_.getSegment(i).getJoint().getName(); - if (std::find(msg->name.begin(), msg->name.end(), joint_name) != msg->name.end()) + KDL::Joint joint = chain_.getSegment(i).getJoint(); + std::string joint_name = joint.getName(); + if (joint.getType() == KDL::Joint::None) { - unsigned int index = std::distance(msg->name.begin(), std::find(msg->name.begin(), msg->name.end(), joint_name)); - q(index) = msg->position[index]; - q_dot(index) = msg->velocity[index]; + ROS_DEBUG_STREAM("Skip fixed Joint '" << joint_name); + continue; } - else + else { - ROS_ERROR_STREAM("Joint '" << joint_name << "' not found in JointStates"); - return; + if (std::find(msg->name.begin(), msg->name.end(), joint_name) != msg->name.end()) + { + unsigned int index = std::distance(msg->name.begin(), std::find(msg->name.begin(), msg->name.end(), joint_name)); + q(index) = msg->position[index]; + q_dot(index) = msg->velocity[index]; + } + else + { + ROS_ERROR_STREAM("Joint '" << joint_name << "' not found in JointStates"); + return; + } } } /// compute current twist std::vector< KDL::FrameVel > v_FrameVel; KDL::JntArrayVel jntArrayVel = KDL::JntArrayVel(q, q_dot); - v_FrameVel.resize(chain_.getNrOfJoints()); - if (p_fksolver_vel_->JntToCart(jntArrayVel, v_FrameVel, chain_.getNrOfJoints()) >= 0) + v_FrameVel.resize(chain_.getNrOfSegments()); + if (p_fksolver_vel_->JntToCart(jntArrayVel, v_FrameVel, chain_.getNrOfSegments()) >= 0) { //last entry is twist of tip_link geometry_msgs::Twist twist_msg; @@ -146,7 +155,7 @@ class DebugEvaluateJointStates //recursively calculate FrameVel magnitudes sensor_msgs::JointState magnitude_msg; magnitude_msg.header.stamp = msg->header.stamp; - for (unsigned int i = 0; i < chain_.getNrOfJoints(); i++) + for (unsigned int i = 0; i < chain_.getNrOfSegments(); i++) { magnitude_msg.name.push_back(chain_.getSegment(i).getName()); magnitude_msg.velocity.push_back(v_FrameVel[i].GetTwist().vel.Norm()); From cf7f50b86469d8b3a4831276e8a3e7952cea9382 Mon Sep 17 00:00:00 2001 From: fmessmer Date: Fri, 26 Nov 2021 10:41:54 +0100 Subject: [PATCH 6/8] rename debug node --- cob_twist_controller/CMakeLists.txt | 8 ++++---- ...ntstates.launch => debug_fk_vel_recursive.launch} | 2 +- ...ates_node.cpp => debug_fk_vel_recursive_node.cpp} | 12 ++++++------ 3 files changed, 11 insertions(+), 11 deletions(-) rename cob_twist_controller/launch/{debug_evaluate_jointstates.launch => debug_fk_vel_recursive.launch} (67%) rename cob_twist_controller/src/debug/{debug_evaluate_jointstates_node.cpp => debug_fk_vel_recursive_node.cpp} (96%) diff --git a/cob_twist_controller/CMakeLists.txt b/cob_twist_controller/CMakeLists.txt index 664babe48..3ae97309d 100644 --- a/cob_twist_controller/CMakeLists.txt +++ b/cob_twist_controller/CMakeLists.txt @@ -73,9 +73,9 @@ add_executable(debug_trajectory_marker_node src/debug/debug_trajectory_marker_no add_dependencies(debug_trajectory_marker_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(debug_trajectory_marker_node ${catkin_LIBRARIES}) -add_executable(debug_evaluate_jointstates_node src/debug/debug_evaluate_jointstates_node.cpp) -add_dependencies(debug_evaluate_jointstates_node ${catkin_EXPORTED_TARGETS}) -target_link_libraries(debug_evaluate_jointstates_node ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) +add_executable(debug_fk_vel_recursive_node src/debug/debug_fk_vel_recursive_node.cpp) +add_dependencies(debug_fk_vel_recursive_node ${catkin_EXPORTED_TARGETS}) +target_link_libraries(debug_fk_vel_recursive_node ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) add_executable(test_moving_average_node src/debug/test_moving_average_node.cpp) add_dependencies(test_moving_average_node ${catkin_EXPORTED_TARGETS}) @@ -106,7 +106,7 @@ install(TARGETS ${PROJECT_NAME}_node constraint_solvers controller_interfaces da RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -install(TARGETS debug_evaluate_jointstates_node debug_trajectory_marker_node test_moving_average_node test_simpson_integrator_node test_trajectory_command_sine_node test_twist_command_sine_node +install(TARGETS debug_fk_vel_recursive_node debug_trajectory_marker_node test_moving_average_node test_simpson_integrator_node test_trajectory_command_sine_node test_twist_command_sine_node ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/cob_twist_controller/launch/debug_evaluate_jointstates.launch b/cob_twist_controller/launch/debug_fk_vel_recursive.launch similarity index 67% rename from cob_twist_controller/launch/debug_evaluate_jointstates.launch rename to cob_twist_controller/launch/debug_fk_vel_recursive.launch index 4c887a246..f02716d6b 100644 --- a/cob_twist_controller/launch/debug_evaluate_jointstates.launch +++ b/cob_twist_controller/launch/debug_fk_vel_recursive.launch @@ -8,7 +8,7 @@ - + diff --git a/cob_twist_controller/src/debug/debug_evaluate_jointstates_node.cpp b/cob_twist_controller/src/debug/debug_fk_vel_recursive_node.cpp similarity index 96% rename from cob_twist_controller/src/debug/debug_evaluate_jointstates_node.cpp rename to cob_twist_controller/src/debug/debug_fk_vel_recursive_node.cpp index 03ccce839..c43235c05 100644 --- a/cob_twist_controller/src/debug/debug_evaluate_jointstates_node.cpp +++ b/cob_twist_controller/src/debug/debug_fk_vel_recursive_node.cpp @@ -34,7 +34,7 @@ #include #include -class DebugEvaluateJointStates +class DebugFkVelRecursive { ros::NodeHandle nh_; ros::Subscriber jointstate_sub_; @@ -97,7 +97,7 @@ class DebugEvaluateJointStates p_jnt2jac_ = new KDL::ChainJntToJacSolver(chain_); /// initialize ROS interfaces - jointstate_sub_ = nh_.subscribe("joint_states", 1, &DebugEvaluateJointStates::jointstateCallback, this); + jointstate_sub_ = nh_.subscribe("joint_states", 1, &DebugFkVelRecursive::jointstateCallback, this); manipulability_pub_ = nh_.advertise ("debug/manipulability", 1); twist_current_pub_ = nh_.advertise ("debug/twist_current", 1); twist_magnitude_pub_ = nh_.advertise ("debug/twist_magnitude", 1); @@ -178,12 +178,12 @@ class DebugEvaluateJointStates int main(int argc, char** argv) { - ros::init(argc, argv, "debug_evaluate_jointstates_node"); + ros::init(argc, argv, "debug_fk_vel_recursive_node"); - DebugEvaluateJointStates dejs; - if (dejs.init() != 0) + DebugFkVelRecursive dfvr; + if (dfvr.init() != 0) { - ROS_ERROR("Failed to initialize DebugEvaluateJointStates."); + ROS_ERROR("Failed to initialize DebugFkVelRecursive."); return -1; } From 0fb2b860dc25f9767645a2a49565936b631a76cc Mon Sep 17 00:00:00 2001 From: fmessmer Date: Fri, 26 Nov 2021 11:46:30 +0100 Subject: [PATCH 7/8] calculate fk_vel_recursive based on joint_trajectory_controller state for both desired and actual joint states --- cob_twist_controller/package.xml | 1 + .../src/debug/debug_fk_vel_recursive_node.cpp | 91 ++++++++++++++++--- 2 files changed, 80 insertions(+), 12 deletions(-) diff --git a/cob_twist_controller/package.xml b/cob_twist_controller/package.xml index 03d5655a2..c7b83f8ea 100644 --- a/cob_twist_controller/package.xml +++ b/cob_twist_controller/package.xml @@ -31,6 +31,7 @@ cmake_modules cob_control_msgs cob_srvs + control_msgs dynamic_reconfigure eigen_conversions eigen diff --git a/cob_twist_controller/src/debug/debug_fk_vel_recursive_node.cpp b/cob_twist_controller/src/debug/debug_fk_vel_recursive_node.cpp index c43235c05..5847f917d 100644 --- a/cob_twist_controller/src/debug/debug_fk_vel_recursive_node.cpp +++ b/cob_twist_controller/src/debug/debug_fk_vel_recursive_node.cpp @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -37,10 +38,12 @@ class DebugFkVelRecursive { ros::NodeHandle nh_; - ros::Subscriber jointstate_sub_; + ros::Subscriber joint_states_sub_; + ros::Subscriber controller_state_sub_; ros::Publisher manipulability_pub_; ros::Publisher twist_current_pub_; - ros::Publisher twist_magnitude_pub_; + ros::Publisher twist_magnitude_desired_pub_; + ros::Publisher twist_magnitude_actual_pub_; std::string chain_base_link_; std::string chain_tip_link_; @@ -97,25 +100,22 @@ class DebugFkVelRecursive p_jnt2jac_ = new KDL::ChainJntToJacSolver(chain_); /// initialize ROS interfaces - jointstate_sub_ = nh_.subscribe("joint_states", 1, &DebugFkVelRecursive::jointstateCallback, this); + // joint_states_sub_ = nh_.subscribe("joint_states", 1, &DebugFkVelRecursive::jointstatesCallback, this); // analyzing controller_state instead + controller_state_sub_ = nh_.subscribe("joint_trajectory_controller/state", 1, &DebugFkVelRecursive::controllerstateCallback, this); manipulability_pub_ = nh_.advertise ("debug/manipulability", 1); twist_current_pub_ = nh_.advertise ("debug/twist_current", 1); - twist_magnitude_pub_ = nh_.advertise ("debug/twist_magnitude", 1); + twist_magnitude_desired_pub_ = nh_.advertise ("debug/twist_magnitude/desired", 1); + twist_magnitude_actual_pub_ = nh_.advertise ("debug/twist_magnitude/actual", 1); return 0; } - void jointstateCallback(const sensor_msgs::JointState::ConstPtr& msg) + void jointstatesCallback(const sensor_msgs::JointState::ConstPtr& msg) { KDL::JntArray q = KDL::JntArray(chain_.getNrOfJoints()); KDL::JntArray q_dot = KDL::JntArray(chain_.getNrOfJoints()); - // //TODO: sort wrt kinematic order - // for (unsigned int i = 0; i < msg->name.size(); i++) - // { - // q(i) = msg->position[i]; - // q_dot(i) = msg->velocity[i]; - // } + /// extract q, q_dot from JointStates for (unsigned int i = 0; i < chain_.getNrOfSegments(); i++) { KDL::Joint joint = chain_.getSegment(i).getJoint(); @@ -160,7 +160,7 @@ class DebugFkVelRecursive magnitude_msg.name.push_back(chain_.getSegment(i).getName()); magnitude_msg.velocity.push_back(v_FrameVel[i].GetTwist().vel.Norm()); } - twist_magnitude_pub_.publish(magnitude_msg); + twist_magnitude_actual_pub_.publish(magnitude_msg); } /// compute manipulability @@ -173,6 +173,73 @@ class DebugFkVelRecursive manipulability_msg.data = kappa; manipulability_pub_.publish(manipulability_msg); } + + void controllerstateCallback(const control_msgs::JointTrajectoryControllerState::ConstPtr& msg) + { + KDL::JntArray q_desired = KDL::JntArray(chain_.getNrOfJoints()); + KDL::JntArray q_desired_dot = KDL::JntArray(chain_.getNrOfJoints()); + KDL::JntArray q_actual = KDL::JntArray(chain_.getNrOfJoints()); + KDL::JntArray q_actual_dot = KDL::JntArray(chain_.getNrOfJoints()); + + for (unsigned int i = 0; i < chain_.getNrOfSegments(); i++) + { + KDL::Joint joint = chain_.getSegment(i).getJoint(); + std::string joint_name = joint.getName(); + if (joint.getType() == KDL::Joint::None) + { + ROS_DEBUG_STREAM("Skip fixed Joint '" << joint_name); + continue; + } + else + { + if (std::find(msg->joint_names.begin(), msg->joint_names.end(), joint_name) != msg->joint_names.end()) + { + unsigned int index = std::distance(msg->joint_names.begin(), std::find(msg->joint_names.begin(), msg->joint_names.end(), joint_name)); + q_desired(index) = msg->desired.positions[index]; + q_desired_dot(index) = msg->desired.velocities[index]; + q_actual(index) = msg->actual.positions[index]; + q_actual_dot(index) = msg->actual.velocities[index]; + } + else + { + ROS_ERROR_STREAM("Joint '" << joint_name << "' not found in JointTrajectoryControllerState"); + return; + } + } + } + + /// compute desired twists recursively + std::vector< KDL::FrameVel > v_FrameVel_desired; + KDL::JntArrayVel jntArrayVel_desired = KDL::JntArrayVel(q_desired, q_desired_dot); + v_FrameVel_desired.resize(chain_.getNrOfSegments()); + if (p_fksolver_vel_->JntToCart(jntArrayVel_desired, v_FrameVel_desired, chain_.getNrOfSegments()) >= 0) + { + sensor_msgs::JointState magnitude_desired_msg; + magnitude_desired_msg.header.stamp = msg->header.stamp; + for (unsigned int i = 0; i < chain_.getNrOfSegments(); i++) + { + magnitude_desired_msg.name.push_back(chain_.getSegment(i).getName()); + magnitude_desired_msg.velocity.push_back(v_FrameVel_desired[i].GetTwist().vel.Norm()); + } + twist_magnitude_desired_pub_.publish(magnitude_desired_msg); + } + /// compute actual twists recursively + std::vector< KDL::FrameVel > v_FrameVel_actual; + KDL::JntArrayVel jntArrayVel_actual = KDL::JntArrayVel(q_actual, q_actual_dot); + v_FrameVel_actual.resize(chain_.getNrOfSegments()); + if (p_fksolver_vel_->JntToCart(jntArrayVel_actual, v_FrameVel_actual, chain_.getNrOfSegments()) >= 0) + { + sensor_msgs::JointState magnitude_actual_msg; + magnitude_actual_msg.header.stamp = msg->header.stamp; + for (unsigned int i = 0; i < chain_.getNrOfSegments(); i++) + { + magnitude_actual_msg.name.push_back(chain_.getSegment(i).getName()); + magnitude_actual_msg.velocity.push_back(v_FrameVel_actual[i].GetTwist().vel.Norm()); + } + twist_magnitude_actual_pub_.publish(magnitude_actual_msg); + } + } + }; From d32cf9ee9055d1816382fec66d3968acb95e4791 Mon Sep 17 00:00:00 2001 From: fmessmer Date: Tue, 30 Nov 2021 17:39:22 +0100 Subject: [PATCH 8/8] fix catkin_lint --- cob_twist_controller/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cob_twist_controller/CMakeLists.txt b/cob_twist_controller/CMakeLists.txt index 3ae97309d..301f00c16 100644 --- a/cob_twist_controller/CMakeLists.txt +++ b/cob_twist_controller/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.0.2) project(cob_twist_controller) -find_package(catkin REQUIRED COMPONENTS cmake_modules cob_control_msgs cob_srvs dynamic_reconfigure eigen_conversions geometry_msgs kdl_conversions kdl_parser nav_msgs pluginlib roscpp roslint sensor_msgs std_msgs tf tf_conversions trajectory_msgs urdf visualization_msgs) +find_package(catkin REQUIRED COMPONENTS cmake_modules cob_control_msgs cob_srvs control_msgs dynamic_reconfigure eigen_conversions geometry_msgs kdl_conversions kdl_parser nav_msgs pluginlib roscpp roslint sensor_msgs std_msgs tf tf_conversions trajectory_msgs urdf visualization_msgs) find_package(Boost REQUIRED COMPONENTS thread) @@ -17,7 +17,7 @@ generate_dynamic_reconfigure_options( ) catkin_package( - CATKIN_DEPENDS cob_control_msgs cob_srvs dynamic_reconfigure eigen_conversions geometry_msgs kdl_conversions kdl_parser nav_msgs pluginlib roscpp sensor_msgs std_msgs tf tf_conversions trajectory_msgs urdf visualization_msgs + CATKIN_DEPENDS cob_control_msgs cob_srvs control_msgs dynamic_reconfigure eigen_conversions geometry_msgs kdl_conversions kdl_parser nav_msgs pluginlib roscpp sensor_msgs std_msgs tf tf_conversions trajectory_msgs urdf visualization_msgs DEPENDS Boost INCLUDE_DIRS include LIBRARIES damping_methods inv_calculations constraint_solvers limiters controller_interfaces kinematic_extensions inverse_differential_kinematics_solver twist_controller