diff --git a/cob_twist_controller/CMakeLists.txt b/cob_twist_controller/CMakeLists.txt index 664babe48..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 @@ -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_fk_vel_recursive.launch b/cob_twist_controller/launch/debug_fk_vel_recursive.launch new file mode 100644 index 000000000..f02716d6b --- /dev/null +++ b/cob_twist_controller/launch/debug_fk_vel_recursive.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + 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_evaluate_jointstates_node.cpp b/cob_twist_controller/src/debug/debug_evaluate_jointstates_node.cpp deleted file mode 100644 index 60d73330a..000000000 --- a/cob_twist_controller/src/debug/debug_evaluate_jointstates_node.cpp +++ /dev/null @@ -1,136 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include - -class DebugEvaluateJointStates -{ - ros::NodeHandle nh_; - ros::Subscriber jointstate_sub_; - ros::Publisher manipulability_pub_; - ros::Publisher twist_current_pub_; - - std::string chain_base_link_; - std::string chain_tip_link_; - - KDL::Chain chain_; - KDL::ChainFkSolverVel_recursive* p_fksolver_vel_; - KDL::ChainJntToJacSolver* p_jnt2jac_; - -public: - int init() - { - if (!nh_.getParam("chain_base_link", this->chain_base_link_)) - { - ROS_ERROR("Failed to get parameter \"chain_base_link\"."); - return -1; - } - - if (!nh_.getParam("chain_tip_link", this->chain_tip_link_)) - { - ROS_ERROR("Failed to get parameter \"chain_tip_link\"."); - return -2; - } - - /// parse robot_description and generate KDL chains - KDL::Tree my_tree; - if (!kdl_parser::treeFromParam("/robot_description", my_tree)) - { - ROS_ERROR("Failed to construct kdl tree"); - return -3; - } - - my_tree.getChain(this->chain_base_link_, this->chain_tip_link_, chain_); - if (chain_.getNrOfJoints() == 0) - { - ROS_ERROR("Failed to initialize kinematic chain"); - return -4; - } - - p_fksolver_vel_ = new KDL::ChainFkSolverVel_recursive(chain_); - p_jnt2jac_ = new KDL::ChainJntToJacSolver(chain_); - - /// initialize ROS interfaces - 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); - - return 0; - } - - void jointstateCallback(const sensor_msgs::JointState::ConstPtr& msg) - { - 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++) - { - q(i) = msg->position[i]; - q_dot(i) = msg->velocity[i]; - } - - /// compute current twist - KDL::FrameVel FrameVel; - KDL::JntArrayVel jntArrayVel = KDL::JntArrayVel(q, q_dot); - if (p_fksolver_vel_->JntToCart(jntArrayVel, FrameVel, -1) >= 0) - { - geometry_msgs::Twist twist_msg; - tf::twistKDLToMsg(FrameVel.GetTwist(), twist_msg); - twist_current_pub_.publish(twist_msg); - } - - /// compute manipulability - KDL::Jacobian jac(chain_.getNrOfJoints()); - p_jnt2jac_->JntToJac(q, jac); - Eigen::Matrix prod = jac.data * jac.data.transpose(); - double d = prod.determinant(); - double kappa = std::sqrt(std::abs(d)); - std_msgs::Float64 manipulability_msg; - manipulability_msg.data = kappa; - manipulability_pub_.publish(manipulability_msg); - } -}; - - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "debug_evaluate_jointstates_node"); - - DebugEvaluateJointStates dejs; - if (dejs.init() != 0) - { - ROS_ERROR("Failed to initialize DebugEvaluateJointStates."); - return -1; - } - - ros::spin(); -} 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 new file mode 100644 index 000000000..5847f917d --- /dev/null +++ b/cob_twist_controller/src/debug/debug_fk_vel_recursive_node.cpp @@ -0,0 +1,258 @@ +/* + * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +class DebugFkVelRecursive +{ + ros::NodeHandle nh_; + ros::Subscriber joint_states_sub_; + ros::Subscriber controller_state_sub_; + ros::Publisher manipulability_pub_; + ros::Publisher twist_current_pub_; + ros::Publisher twist_magnitude_desired_pub_; + ros::Publisher twist_magnitude_actual_pub_; + + std::string chain_base_link_; + std::string chain_tip_link_; + + KDL::Chain chain_; + KDL::ChainFkSolverVel_recursive* p_fksolver_vel_; + KDL::ChainJntToJacSolver* p_jnt2jac_; + +public: + int init() + { + if (!nh_.getParam("chain_base_link", this->chain_base_link_)) + { + ROS_ERROR("Failed to get parameter \"chain_base_link\"."); + return -1; + } + + if (!nh_.getParam("chain_tip_link", this->chain_tip_link_)) + { + ROS_ERROR("Failed to get parameter \"chain_tip_link\"."); + return -2; + } + + /// parse robot_description and generate KDL chains + KDL::Tree my_tree; + if (!kdl_parser::treeFromParam("/robot_description", my_tree)) + { + ROS_ERROR("Failed to construct kdl tree"); + return -3; + } + + my_tree.getChain(this->chain_base_link_, this->chain_tip_link_, chain_); + if (chain_.getNrOfJoints() == 0) + { + ROS_ERROR("Failed to initialize kinematic chain"); + return -4; + } + + //debug chain + ROS_WARN("~~~~~~~~~~~~~ DEBUG CHAIN ~~~~~~~~~~~~~"); + ROS_WARN_STREAM("NrOfJoints: "< ("debug/manipulability", 1); + twist_current_pub_ = nh_.advertise ("debug/twist_current", 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 jointstatesCallback(const sensor_msgs::JointState::ConstPtr& msg) + { + KDL::JntArray q = KDL::JntArray(chain_.getNrOfJoints()); + KDL::JntArray q_dot = KDL::JntArray(chain_.getNrOfJoints()); + + /// extract q, q_dot from JointStates + 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->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_.getNrOfSegments()); + if (p_fksolver_vel_->JntToCart(jntArrayVel, v_FrameVel, chain_.getNrOfSegments()) >= 0) + { + //last entry is twist of tip_link + geometry_msgs::Twist 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; + magnitude_msg.header.stamp = msg->header.stamp; + 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()); + } + twist_magnitude_actual_pub_.publish(magnitude_msg); + } + + /// compute manipulability + KDL::Jacobian jac(chain_.getNrOfJoints()); + p_jnt2jac_->JntToJac(q, jac); + Eigen::Matrix prod = jac.data * jac.data.transpose(); + double d = prod.determinant(); + double kappa = std::sqrt(std::abs(d)); + std_msgs::Float64 manipulability_msg; + 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); + } + } + +}; + + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "debug_fk_vel_recursive_node"); + + DebugFkVelRecursive dfvr; + if (dfvr.init() != 0) + { + ROS_ERROR("Failed to initialize DebugFkVelRecursive."); + return -1; + } + + ros::spin(); +}