From cba4b0ed69b9ef2e6086e5f815f2e22376985c8a Mon Sep 17 00:00:00 2001 From: Niko Date: Thu, 1 Feb 2024 19:54:30 +0100 Subject: [PATCH] task not working --- CMakeLists.txt | 18 ++- config/panda.srdf | 120 ++++++++++++++++ launch/pose_recoder.launch.py | 144 +++++++++++++++++++ launch/run_grasp.launch.py | 0 src/grasp_interface.cpp | 262 +++++++++++++++++++--------------- src/moveit_interface.cpp | 1 + src/pose_record.cpp | 77 ++++++++++ 7 files changed, 506 insertions(+), 116 deletions(-) create mode 100644 config/panda.srdf create mode 100755 launch/pose_recoder.launch.py create mode 100644 launch/run_grasp.launch.py create mode 100644 src/pose_record.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 38342e7..a455864 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,11 +30,12 @@ find_package(geometry_msgs REQUIRED) find_package(franka_cps_msgs REQUIRED) find_package(franka_msgs REQUIRED) -find_package(tf2) -find_package(tf2_ros) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) add_executable(franka_iml_interface src/moveit_interface.cpp) add_executable(franka_grasp_interface src/grasp_interface.cpp) +add_executable(franka_pose_recorder src/pose_record.cpp) ament_target_dependencies( franka_iml_interface @@ -66,12 +67,25 @@ ament_target_dependencies( tf2_ros ) +ament_target_dependencies( + franka_pose_recorder + rclcpp + geometry_msgs + tf2 + tf2_ros +) + install( TARGETS franka_iml_interface TARGETS franka_grasp_interface + TARGETS franka_pose_recorder DESTINATION lib/${PROJECT_NAME} ) +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/config/panda.srdf b/config/panda.srdf new file mode 100644 index 0000000..6433749 --- /dev/null +++ b/config/panda.srdf @@ -0,0 +1,120 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/pose_recoder.launch.py b/launch/pose_recoder.launch.py new file mode 100755 index 0000000..1e329f4 --- /dev/null +++ b/launch/pose_recoder.launch.py @@ -0,0 +1,144 @@ + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import (DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, + Shutdown) +from launch.conditions import UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import yaml + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, 'r') as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + robot_ip_parameter_name = 'robot_ip' + use_fake_hardware_parameter_name = 'use_fake_hardware' + fake_sensor_commands_parameter_name = 'fake_sensor_commands' + + robot_ip = LaunchConfiguration(robot_ip_parameter_name) + use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name) + fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name) + + # planning_context + franka_xacro_file = os.path.join(get_package_share_directory('franka_description'), 'robots', + 'panda_arm.urdf.xacro') + robot_description_config = Command( + [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=true', + ' robot_ip:=', robot_ip, ' use_fake_hardware:=', use_fake_hardware, + ' fake_sensor_commands:=', fake_sensor_commands]) + + robot_description = {'robot_description': robot_description_config} + + franka_semantic_xacro_file = os.path.join(get_package_share_directory('franka_moveit_config'), + 'srdf', + 'panda_arm.srdf.xacro') + # Publish TF + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='both', + parameters=[robot_description], + ) + + ros2_controllers_path = os.path.join( + get_package_share_directory('franka_moveit_config'), + 'config', + 'panda_ros_controllers.yaml', + ) + ros2_control_node = Node( + package='controller_manager', + executable='ros2_control_node', + parameters=[robot_description, ros2_controllers_path], + remappings=[('joint_states', 'franka/joint_states')], + output={ + 'stdout': 'screen', + 'stderr': 'screen', + }, + on_exit=Shutdown(), + ) + + load_controllers = [] + for controller in ['joint_state_broadcaster']: + load_controllers += [ + ExecuteProcess( + cmd=['ros2 run controller_manager spawner {}'.format(controller)], + shell=True, + output='screen', + ) + ] + + joint_state_publisher = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + parameters=[ + {'source_list': ['franka/joint_states', 'panda_gripper/joint_states'], 'rate': 30}], + ) + + franka_robot_state_broadcaster = Node( + package='controller_manager', + executable='spawner', + arguments=['franka_robot_state_broadcaster'], + output='screen', + condition=UnlessCondition(use_fake_hardware), + ) + + + franka_pose_recorder =Node( + package='franka_moveit_interface', + executable='franka_pose_recorder', + name='franka_pose_recorder', + output='screen', + parameters=[robot_description] + ) + + + robot_arg = DeclareLaunchArgument( + robot_ip_parameter_name, + default_value='192.168.1.211', + description='Hostname or IP address of the robot.') + + use_fake_hardware_arg = DeclareLaunchArgument( + use_fake_hardware_parameter_name, + default_value='false', + description='Use fake hardware') + fake_sensor_commands_arg = DeclareLaunchArgument( + fake_sensor_commands_parameter_name, + default_value='false', + description="Fake sensor commands. Only valid when '{}' is true".format( + use_fake_hardware_parameter_name)) + + gripper_launch_file = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution( + [FindPackageShare('franka_gripper'), 'launch', 'gripper.launch.py'])]), + launch_arguments={'robot_ip': robot_ip, + use_fake_hardware_parameter_name: use_fake_hardware}.items(), + ) + + return LaunchDescription( + [ + robot_arg, + use_fake_hardware_arg, + fake_sensor_commands_arg, + robot_state_publisher, + joint_state_publisher, + franka_pose_recorder, + ros2_control_node, + franka_robot_state_broadcaster + ] + load_controllers + ) \ No newline at end of file diff --git a/launch/run_grasp.launch.py b/launch/run_grasp.launch.py new file mode 100644 index 0000000..e69de29 diff --git a/src/grasp_interface.cpp b/src/grasp_interface.cpp index eb21d95..afc49f4 100644 --- a/src/grasp_interface.cpp +++ b/src/grasp_interface.cpp @@ -9,8 +9,6 @@ #include #include -#include -#include #include #include @@ -18,18 +16,6 @@ #include -#include "tf2_ros/transform_listener.h" - -#if __has_include() -#include -#else -#include -#endif -#if __has_include() -#include -#else -#include -#endif static const rclcpp::Logger LOGGER = rclcpp::get_logger("franka_grasp_interface"); namespace mtc = moveit::task_constructor; @@ -39,11 +25,9 @@ class GraspInterfaceNode : public rclcpp::Node public: using GraspSequence = franka_cps_msgs::action::GraspSequence; using GoalHandleGraspSequence = rclcpp_action::ServerGoalHandle; - using FrankaMove = franka_msgs::action::Move; - using FrankaGrasp = franka_msgs::action::Grasp; GraspInterfaceNode() - : Node("grasp_interface"), tf_buffer_(this->get_clock()),tf_listener_(tf_buffer_) + : Node("grasp_interface") { // Action server for getting new Grasp Sequences action_server_ = rclcpp_action::create_server( @@ -53,60 +37,26 @@ class GraspInterfaceNode : public rclcpp::Node std::bind(&GraspInterfaceNode::handle_cancel, this, std::placeholders::_1), std::bind(&GraspInterfaceNode::handle_accepted, this, std::placeholders::_1)); - // Publisher for pose - pose_publisher_ = this->create_publisher("franka_grasp_moveit/current_pose", 10); - - // Service for IK - - - // TF2 - tf2_ros::Buffer tf_buffer_(this->get_clock()); - tf2_ros::TransformListener tf_listener_(tf_buffer_); - tf2_timer_ = this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&GraspInterfaceNode::compute_endeffector_pose, this)); - // Setup for move groups static const std::string arm_group_name = "panda_arm"; - static const std::string hand_group_name = "hand"; - static const std::string hand_frame = "panda_hand"; + static const std::string hand_group_name = "hand"; + static const std::string hand_frame = "panda_link8"; + static const std::string eef_name = "hand"; + + } private: rclcpp_action::Server::SharedPtr action_server_; - rclcpp::Publisher::SharedPtr pose_publisher_; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; - rclcpp::TimerBase::SharedPtr tf2_timer_; - - void compute_endeffector_pose() - { - try - { - geometry_msgs::msg::TransformStamped transform_stamped; - transform_stamped = tf_buffer_.lookupTransform("panda_link1", "panda_link7", rclcpp::Time(0)); - - geometry_msgs::msg::PoseStamped current_pose; - current_pose.header.frame_id = "panda_link8"; - current_pose.header.stamp = transform_stamped.header.stamp; - //Position - current_pose.pose.position.x = transform_stamped.transform.translation.x; - current_pose.pose.position.y = transform_stamped.transform.translation.y; - current_pose.pose.position.z = transform_stamped.transform.translation.z; - // Orientation - current_pose.pose.orientation.x = transform_stamped.transform.rotation.x; - current_pose.pose.orientation.y = transform_stamped.transform.rotation.y; - current_pose.pose.orientation.z = transform_stamped.transform.rotation.z; - current_pose.pose.orientation.w = transform_stamped.transform.rotation.w; - pose_publisher_->publish(current_pose); - } - catch (tf2::TransformException &ex) - { - RCLCPP_WARN(this->get_logger(), "%s", ex.what()); - } - } + static const std::string arm_group_name; + static const std::string hand_group_name; + static const std::string hand_frame; + static const std::string eef_name; + const std::array tcp_frame = {0, 0, 0.1034, 0, 0, 0}; rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) + std::shared_ptr /*goal*/) { RCLCPP_INFO(this->get_logger(), "Received goal request!"); (void)uuid; @@ -157,68 +107,152 @@ class GraspInterfaceNode : public rclcpp::Node task.loadRobotModel(this->shared_from_this()); // set Planners + auto sampling_planner = std::make_shared(this->shared_from_this()); auto interpolation_planner = std::make_shared(); + auto cartesian_planner = std::make_shared(); + cartesian_planner->setMaxVelocityScalingFactor(1.0); + cartesian_planner->setMaxAccelerationScalingFactor(1.0); + cartesian_planner->setStepSize(.01); - std::vector joint_positions; - /* - // Compute IK for each pose and create corresponding stages - if (computeIK(goal->hover, joint_positions)) { - // Create and add MoveTo stage using computed joint positions - auto move_to_hover = std::make_unique("Move to Hover", interpolation_planner); - move_to_hover->setGroup(arm_group_name); // set group - move_to_hover->setGoal(joint_positions); // set goal - task.add(std::move(move_to_hover)); // add Task - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to compute IK for hover pose"); - } + // Set task properties + task.setProperty("group", arm_group_name); + task.setProperty("eef", eef_name); + task.setProperty("hand", hand_group_name); + task.setProperty("hand_grasping_frame", hand_frame); + task.setProperty("ik_frame", hand_frame); - if (computeIK(goal->grasp, joint_positions)) { - auto move_to_hover = std::make_unique("Move to Hover", interpolation_planner); - move_to_hover->setGroup(arm_group_name); // set group - move_to_hover->setGoal(joint_positions); // set goal - task.add(std::move(move_to_hover)); // add Task - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to compute IK for hover pose"); - } + // Current State + auto current_state = std::make_unique("current state"); - if (computeIK(goal->hover, joint_positions)) { - // Create and add MoveTo stage using computed joint positions - auto move_to_hover2 = std::make_unique("Move to Hover", interpolation_planner); - move_to_hover2->setGroup(arm_group_name); // set group - move_to_hover2->setGoal(joint_positions); // set goal - task.add(std::move(move_to_hover2)); // add Task - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to compute IK for hover pose"); - } + // Open Hand + moveit::task_constructor::Stage* initial_state_ptr = nullptr; - if (computeIK(goal->drop, joint_positions)) { - // Create and add MoveTo stage using computed joint positions - auto move_to_drop = std::make_unique("Move to drop", interpolation_planner); - move_to_drop->setGroup(arm_group_name); // set group - move_to_drop->setGoal(joint_positions); // set goal - task.add(std::move(move_to_drop)); // add Task - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to compute IK for drop pose"); - } + auto stage1 = std::make_unique("open hand", sampling_planner); + stage1.setGroup(hand_group_name); + stage1.setGoal("open"); + initial_state_ptr = stage1.get(); + task.add(std::move(stage1)); - if (computeIK(goal->idle, joint_positions)) { - // Create and add MoveTo stage using computed joint positions - auto move_to_idle = std::make_unique("Move to idle", interpolation_planner); - move_to_idle->setGroup(arm_group_name); // set group - move_to_idle->setGoal(joint_positions); // set goal - task.add(std::move(move_to_idle)); // add Task - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to compute IK for idle pose"); - } - */ + + + // Grasping move + auto grasp = std::make_unique("pick object"); + task.properties().exposeTo(grasp.properties(), { "eef", "hand", "group", "ik_frame" }); + grasp.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group", "ik_frame" }); + + // Move Hower + auto stage2 = std::make_unique("generate hower pose"); + stage2.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT); + stage2.properties().set("marker_ns", "hower_pose"); + stage2.setPose(goal.hower) + stage2.setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions + + // Compute IK Hower + auto wrapper1 = std::make_unique("hower pose IK", std::move(stage)); + wrapper1.setMaxIKSolutions(2); + wrapper1.setMinSolutionDistance(1.0); + wrapper1.setIKFrame(vectorToEigen(tcp_frame), hand_frame); + wrapper1.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" }); + wrapper1.properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" }); + grasp.insert(std::move(wrapper1)); + + // Move Grasp + auto stage3 = std::make_unique("generate grasp pose"); + stage3.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT); + stage3.properties().set("marker_ns", "grasp_pose"); + stage3.setPose(goal.grasp) + stage3.setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions + + // Compute IK Grasp + auto wrapper2 = std::make_unique("grasp pose IK", std::move(stage)); + wrapper2.setMaxIKSolutions(2); + wrapper2.setMinSolutionDistance(1.0); + wrapper2.setIKFrame(vectorToEigen(tcp_frame), hand_frame); + wrapper2.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" }); + wrapper2.properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" }); + grasp.insert(std::move(wrapper2)); + + // Allow Collision + auto stage4 = std::make_unique("allow collision (hand,object)"); + stage4.allowCollisions( + "object", + task.getRobotModel().getJointModelGroup(hand_group_name).getLinkModelNamesWithCollisionGeometry(), + true); + grasp.insert(std::move(stage4)); + + // Close Hand + auto stage5 = std::make_unique("close hand", sampling_planner); + stage5.setGroup(hand_group_name); + stage5.setGoal("close"); + grasp.insert(std::move(stage5)); + + // Move Back to Hower + auto stage6 = std::make_unique("generate hower pose"); + stage6.properties().configureInitFrom(Stage::PARENT); + stage6.properties().set("marker_ns", "hower_pose"); + stage6.setPose(goal.hower) + stage6.setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions + + // Compute IK Hower + grasp.insert(std::move(wrapper2)); + + // Transition to Drop Phase + task.add(std::move(grasp)); + + + // Move Drop + auto stage7 = std::make_unique("generate hower pose"); + stage7.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT); + stage7.properties().set("marker_ns", "hower_pose"); + stage7.setPose(goal.drop) + stage7.setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions + + // Compute IK Drop + auto wrapper3 = std::make_unique("drop pose IK", std::move(stage)); + wrapper3.setMaxIKSolutions(2); + wrapper3.setMinSolutionDistance(1.0); + wrapper3.setIKFrame(vectorToEigen(tcp_frame), hand_frame); + wrapper3.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" }); + wrapper3.properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" }); + task.add(std::move(wrapper3)); + + // Open Hand + auto stage8 = std::make_unique("open hand", sampling_planner); + stage8.setGroup(hand_group_name); + stage8.setGoal("open"); + task.add(std::move(stage8)); + + // Move Idle + auto stage9 = std::make_unique("generate idle pose"); + stage9.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT); + stage9.properties().set("marker_ns", "hower_pose"); + stage9.setPose(goal.idle) + stage9.setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions + + // Compute IK Idle + auto wrapper4 = std::make_unique("drop pose IK", std::move(stage)); + wrapper4.setMaxIKSolutions(2); + wrapper4.setMinSolutionDistance(1.0); + wrapper4.setIKFrame(vectorToEigen(tcp_frame), hand_frame); + wrapper4.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" }); + wrapper4.properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" }); + task.add(std::move(wrapper4)); return task; } - bool executeTask(moveit::task_constructor::Task& task) { - // Logic to execute the given task - // Return true if successful, false otherwise - return 1; + bool executeTask(moveit::task_constructor::Task::Task task) { + RCLCPP_INFO(LOGGER, "Executing solution trajectory"); + moveit_msgs::msg::MoveItErrorCodes execute_result; + + execute_result = task.execute(*task.solutions().front()); + + if (execute_result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { + RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed and returned: " << execute_result.val); + return false; + } + + return true; } }; diff --git a/src/moveit_interface.cpp b/src/moveit_interface.cpp index 147b7ff..cda2f2d 100644 --- a/src/moveit_interface.cpp +++ b/src/moveit_interface.cpp @@ -22,6 +22,7 @@ #include "moveit/planning_scene_interface/planning_scene_interface.h" #include "tf2_ros/transform_listener.h" +#include using namespace std::chrono_literals; diff --git a/src/pose_record.cpp b/src/pose_record.cpp new file mode 100644 index 0000000..7571bcd --- /dev/null +++ b/src/pose_record.cpp @@ -0,0 +1,77 @@ +#include +#include +#include +#include +#include + +#include + +#include +#include "geometry_msgs/msg/transform_stamped.h" + +#include "tf2_ros/transform_listener.h" +#include + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("franka_pose_recorder"); +using namespace std::chrono_literals; + +class PoseRecoderNode : public rclcpp::Node +{ + public: + using PoseStampedMsg = geometry_msgs::msg::PoseStamped; + + PoseRecoderNode() + : Node("pose_recoder"), tf_buffer_(this->get_clock()),tf_listener_(tf_buffer_) + { + // Publisher for pose + pose_publisher_ = this->create_publisher("pose_recoder/current_pose", 10); + + + // TF2 + tf2_ros::Buffer tf_buffer_{this->get_clock()}; + tf2_ros::TransformListener tf_listener_{tf_buffer_}; + tf2_timer_ = this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&PoseRecoderNode::compute_endeffector_pose, this)); + + } + + private: + rclcpp::Publisher::SharedPtr pose_publisher_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + rclcpp::TimerBase::SharedPtr tf2_timer_; + + void compute_endeffector_pose() + { + try + { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped = tf_buffer_.lookupTransform("panda_link1", "panda_link7", rclcpp::Time(0)); + + geometry_msgs::msg::PoseStamped current_pose; + current_pose.header.frame_id = "panda_link8"; + current_pose.header.stamp = transform_stamped.header.stamp; + //Position + current_pose.pose.position.x = transform_stamped.transform.translation.x; + current_pose.pose.position.y = transform_stamped.transform.translation.y; + current_pose.pose.position.z = transform_stamped.transform.translation.z; + // Orientation + current_pose.pose.orientation.x = transform_stamped.transform.rotation.x; + current_pose.pose.orientation.y = transform_stamped.transform.rotation.y; + current_pose.pose.orientation.z = transform_stamped.transform.rotation.z; + current_pose.pose.orientation.w = transform_stamped.transform.rotation.w; + pose_publisher_->publish(current_pose); + } + catch (tf2::TransformException &ex) + { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + } + } +}; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +}