From 95d819a91fffb3855f69d75c88b93f81806b33d0 Mon Sep 17 00:00:00 2001 From: Joseph Coombe Date: Fri, 3 Aug 2018 21:23:39 -0500 Subject: [PATCH 1/2] Crude extension of hebiros_actions to support FollowJointTrajectory Action Goals sent by the moveit_simple_controller_manager/MoveItSimpleControllerManager Plugin --- hebiros/CMakeLists.txt | 2 + hebiros/include/hebiros/hebiros_actions.h | 7 +- hebiros/package.xml | 4 + hebiros/src/hebiros_actions.cpp | 142 ++++++++++++++++++++++ 4 files changed, 154 insertions(+), 1 deletion(-) diff --git a/hebiros/CMakeLists.txt b/hebiros/CMakeLists.txt index fc9c2cb..65ca714 100644 --- a/hebiros/CMakeLists.txt +++ b/hebiros/CMakeLists.txt @@ -13,6 +13,8 @@ find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs + control_msgs + trajectory_msgs tf urdf message_generation diff --git a/hebiros/include/hebiros/hebiros_actions.h b/hebiros/include/hebiros/hebiros_actions.h index 8645080..e80e5ec 100644 --- a/hebiros/include/hebiros/hebiros_actions.h +++ b/hebiros/include/hebiros/hebiros_actions.h @@ -5,7 +5,7 @@ #include "actionlib/server/simple_action_server.h" #include "hebiros/TrajectoryAction.h" - +#include "control_msgs/FollowJointTrajectoryAction.h" class HebirosActions { @@ -15,8 +15,13 @@ class HebirosActions { std::shared_ptr>> trajectory_actions; + static std::map>> + follow_joint_trajectory_actions; + void registerGroupActions(std::string group_name); void trajectory(const hebiros::TrajectoryGoalConstPtr& goal, std::string group_name); + void follow_joint_trajectory(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, std::string group_name); }; diff --git a/hebiros/package.xml b/hebiros/package.xml index c41de22..3dd45a7 100644 --- a/hebiros/package.xml +++ b/hebiros/package.xml @@ -44,6 +44,8 @@ rospy std_msgs sensor_msgs + control_msgs + trajectory_msgs tf actionlib actionlib_msgs @@ -52,6 +54,8 @@ rospy std_msgs sensor_msgs + control_msgs + trajectory_msgs tf ros_control ros_controllers diff --git a/hebiros/src/hebiros_actions.cpp b/hebiros/src/hebiros_actions.cpp index d318535..1a796f1 100644 --- a/hebiros/src/hebiros_actions.cpp +++ b/hebiros/src/hebiros_actions.cpp @@ -6,10 +6,26 @@ using namespace hebiros; +int count_character_occurences(std::string s, char ch) { + int count = 0; + + for (int i = 0; i < s.size(); i++) { + if (s[i] == ch) { + count++; + } + } + + return count; +} + std::map>> HebirosActions::trajectory_actions; +std::map>> + HebirosActions::follow_joint_trajectory_actions; + void HebirosActions::registerGroupActions(std::string group_name) { trajectory_actions[group_name] = std::make_shared< @@ -18,6 +34,14 @@ void HebirosActions::registerGroupActions(std::string group_name) { boost::bind(&HebirosActions::trajectory, this, _1, group_name), false); trajectory_actions[group_name]->start(); + + + follow_joint_trajectory_actions[group_name] = std::make_shared< + actionlib::SimpleActionServer>( + *HebirosNode::n_ptr, "/hebiros/"+group_name+"/follow_joint_trajectory", + boost::bind(&HebirosActions::follow_joint_trajectory, this, _1, group_name), false); + + follow_joint_trajectory_actions[group_name]->start(); } void HebirosActions::trajectory(const TrajectoryGoalConstPtr& goal, std::string group_name) { @@ -117,3 +141,121 @@ void HebirosActions::trajectory(const TrajectoryGoalConstPtr& goal, std::string action_server->setSucceeded(result); ROS_INFO("Group [%s]: Finished executing trajectory", group_name.c_str()); } + +void HebirosActions::follow_joint_trajectory(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, std::string group_name) { + + auto& registry = HebirosGroupRegistry::Instance(); + HebirosGroup* group = registry.getGroup(group_name); + + if (!group) { + ROS_WARN("Group not found."); + return; + } + + std::shared_ptr> action_server = + follow_joint_trajectory_actions[group_name]; + + int num_waypoints = goal->trajectory.points.size(); + if (num_waypoints < 1) { + ROS_WARN("No waypoints sent."); + return; + } + int num_joints = goal->trajectory.joint_names.size(); + ROS_INFO("num_joints [%i]", num_joints); + + Eigen::MatrixXd positions(num_joints, num_waypoints); + Eigen::MatrixXd velocities(num_joints, num_waypoints); + Eigen::MatrixXd accelerations(num_joints, num_waypoints); + Eigen::VectorXd time(num_waypoints); + + for (int i = 0; i < num_waypoints; i++) { + time(i) = goal->trajectory.points[i].time_from_start.toSec(); + ROS_INFO("time [%f]", time(i)); + } + + for (int i = 0; i < num_joints; i++) { + std::string joint_name = goal->trajectory.joint_names[i]; + std::string hebi_joint_name; + if (count_character_occurences(joint_name, '/') > 1) { + int split_index = joint_name.rfind('/'); + hebi_joint_name = joint_name.substr(0, split_index); + } else { + hebi_joint_name = joint_name; + } + ROS_INFO("hebi_joint_name [%s]", hebi_joint_name.c_str()); + + int joint_index = group->joints[hebi_joint_name]; + ROS_INFO("joint_index [%i]", joint_index); + + for (int j = 0; j < num_waypoints; j++) { + double position = goal->trajectory.points[j].positions[i]; + double velocity = goal->trajectory.points[j].velocities[i]; + double acceleration = goal->trajectory.points[j].accelerations[i]; + + positions(joint_index, j) = position; + velocities(joint_index, j) = velocity; + accelerations(joint_index, j) = acceleration; + } + } + + auto trajectory = trajectory::Trajectory::createUnconstrainedQp( + time, positions, &velocities, &accelerations); + Eigen::VectorXd position_command(num_joints); + Eigen::VectorXd velocity_command(num_joints); + + double trajectory_duration = trajectory->getDuration(); + double previous_time; + double current_time; + double loop_duration; + control_msgs::FollowJointTrajectoryFeedback feedback; + + ros::Rate loop_rate(HebirosParameters::getInt("/hebiros/action_frequency")); + + ROS_INFO("Group [%s]: Executing follow_joint_trajectory", group_name.c_str()); + previous_time = ros::Time::now().toSec(); + for (double t = 0; t < trajectory_duration; t += loop_duration) + { + if (action_server->isPreemptRequested() || !ros::ok()) { + ROS_INFO("Group [%s]: Preempted follow_joint_trajectory", group_name.c_str()); + action_server->setPreempted(); + return; + } + + feedback.header.stamp = ros::Time::now(); + feedback.joint_names = goal->trajectory.joint_names; + action_server->publishFeedback(feedback); + + trajectory->getState(t, &position_command, &velocity_command, nullptr); + sensor_msgs::JointState joint_state_msg; + joint_state_msg.name.resize(num_joints); + joint_state_msg.position.resize(num_joints); + joint_state_msg.velocity.resize(num_joints); + + for (int i = 0; i < num_joints; i++) { + std::string joint_name = goal->trajectory.joint_names[i]; + std::string hebi_joint_name; + if (count_character_occurences(joint_name, '/') > 1) { + int split_index = joint_name.rfind('/'); + hebi_joint_name = joint_name.substr(0, split_index); + } else { + hebi_joint_name = joint_name; + } + int joint_index = group->joints[hebi_joint_name]; + joint_state_msg.name[joint_index] = hebi_joint_name; + joint_state_msg.position[joint_index] = position_command(i); + joint_state_msg.velocity[joint_index] = velocity_command(i); + } + HebirosNode::publishers_physical.commandJointState(joint_state_msg, group_name); + + ros::spinOnce(); + loop_rate.sleep(); + current_time = ros::Time::now().toSec(); + loop_duration = current_time - previous_time; + previous_time = current_time; + } + + control_msgs::FollowJointTrajectoryResult result; + result.error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL; + action_server->setSucceeded(result); + ROS_INFO("Group [%s]: Finished executing follow_joint_trajectory", group_name.c_str()); +} From dfb663524ae961418107e946655b227b92552dc8 Mon Sep 17 00:00:00 2001 From: Joseph Coombe Date: Mon, 6 Aug 2018 13:36:42 -0500 Subject: [PATCH 2/2] Fix typo --- hebiros/src/hebiros_actions.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/hebiros/src/hebiros_actions.cpp b/hebiros/src/hebiros_actions.cpp index 1a796f1..b870324 100644 --- a/hebiros/src/hebiros_actions.cpp +++ b/hebiros/src/hebiros_actions.cpp @@ -6,7 +6,7 @@ using namespace hebiros; -int count_character_occurences(std::string s, char ch) { +int count_character_occurrences(std::string s, char ch) { int count = 0; for (int i = 0; i < s.size(); i++) { @@ -176,7 +176,7 @@ void HebirosActions::follow_joint_trajectory(const control_msgs::FollowJointTraj for (int i = 0; i < num_joints; i++) { std::string joint_name = goal->trajectory.joint_names[i]; std::string hebi_joint_name; - if (count_character_occurences(joint_name, '/') > 1) { + if (count_character_occurrences(joint_name, '/') > 1) { int split_index = joint_name.rfind('/'); hebi_joint_name = joint_name.substr(0, split_index); } else { @@ -234,7 +234,7 @@ void HebirosActions::follow_joint_trajectory(const control_msgs::FollowJointTraj for (int i = 0; i < num_joints; i++) { std::string joint_name = goal->trajectory.joint_names[i]; std::string hebi_joint_name; - if (count_character_occurences(joint_name, '/') > 1) { + if (count_character_occurrences(joint_name, '/') > 1) { int split_index = joint_name.rfind('/'); hebi_joint_name = joint_name.substr(0, split_index); } else {