From 1a3bb70f0df2b4d97f78f9e51f715e2e1d5c34e3 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Thu, 13 Feb 2025 14:39:07 +0100 Subject: [PATCH 1/3] yaml file for param tuning added --- ras_moveit/CMakeLists.txt | 12 +- ras_moveit/config/tune_params.yaml | 15 + ras_moveit/include/moveit_server.hpp | 94 --- .../include/ras_moveit/moveit_server.hpp | 119 ++++ .../launch/moveit_real_server.launch.py | 22 +- ras_moveit/launch/moveit_server.launch.py | 22 +- ras_moveit/src/moveit_server.cpp | 548 ++++++------------ ras_moveit/src/moveit_services.cpp | 193 ++++++ 8 files changed, 529 insertions(+), 496 deletions(-) create mode 100644 ras_moveit/config/tune_params.yaml delete mode 100644 ras_moveit/include/moveit_server.hpp create mode 100644 ras_moveit/include/ras_moveit/moveit_server.hpp create mode 100644 ras_moveit/src/moveit_services.cpp diff --git a/ras_moveit/CMakeLists.txt b/ras_moveit/CMakeLists.txt index f3ce378..19fae78 100644 --- a/ras_moveit/CMakeLists.txt +++ b/ras_moveit/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(std_srvs REQUIRED) install( DIRECTORY + config include launch DESTINATION @@ -26,7 +27,11 @@ install( include_directories(include) -add_executable(moveit_server src/moveit_server.cpp) +## build executable +add_executable(moveit_server + src/moveit_server.cpp + src/moveit_services.cpp + ) ament_target_dependencies(moveit_server rclcpp @@ -43,12 +48,7 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() diff --git a/ras_moveit/config/tune_params.yaml b/ras_moveit/config/tune_params.yaml new file mode 100644 index 0000000..dfaffae --- /dev/null +++ b/ras_moveit/config/tune_params.yaml @@ -0,0 +1,15 @@ +moveit_server: + ros__parameters: + collision_object_frame: "world" + base_frame_id: "link_base" + end_effector_frame_id: "link_eef" + orientation_tolerance: + x: 0.75 + y: 0.75 + z: 0.75 + planning_attempts: 5 + planning_time: 2.0 + goal_tolerance: 0.005 + goal_orientation_tolerance: 0.005 + velocity_scaling_factor: 0.2 + acceleration_scaling_factor: 0.4 diff --git a/ras_moveit/include/moveit_server.hpp b/ras_moveit/include/moveit_server.hpp deleted file mode 100644 index 6cce2f1..0000000 --- a/ras_moveit/include/moveit_server.hpp +++ /dev/null @@ -1,94 +0,0 @@ -/* - * - * Copyright (C) 2024 Harsh Davda - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU Affero General Public License as published - * by the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU Affero General Public License for more details. - * - * You should have received a copy of the GNU Affero General Public License - * along with this program. If not, see . - * - * For inquiries or further information, you may contact: - * Harsh Davda - * Email: info@opensciencestack.org -*/ - -#ifndef MOVEIT_SERVER_HPP -#define MOVEIT_SERVER_HPP - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using moveit::planning_interface::MoveGroupInterface; - -class MoveitServer : public rclcpp::Node, public std::enable_shared_from_this -{ -public: - MoveitServer(std::shared_ptr move_group_node); - void move_to_pose_callback(const std::shared_ptr request, std::shared_ptr response); - void move_to_joint_callback(const std::shared_ptr request, std::shared_ptr response); - void rotate_effector_callback(const std::shared_ptr request, std::shared_ptr response); - void joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr msg); - void sync_callback(const std::shared_ptr request,std::shared_ptr response); - void set_constraints(const geometry_msgs::msg::Pose::_orientation_type& quat); - bool Execute(geometry_msgs::msg::Pose target_pose); - bool Execute(sensor_msgs::msg::JointState target_joints); - void trajectory_callback(const std::shared_ptr request, - std::shared_ptr response); - // void trajectory_callback(const std::shared_ptr request, - // std::shared_ptr response); - void AddScenePlane(); - -private: - std::shared_ptr move_group_arm; - moveit::planning_interface::PlanningSceneInterface planning_scene_interface; - // moveit::core::RobotStatePtr current_state_arm; - // const moveit::core::JointModelGroup *joint_model_group_arm; - // moveit::planning_interface::MoveGroupInterface::Plan my_plan_arm; - - rclcpp::Service::SharedPtr move_to_pose_srv_; - rclcpp::Service::SharedPtr move_to_joint_srv_; - rclcpp::Publisher::SharedPtr trajectory_pub; - rclcpp::Service::SharedPtr rotate_effector_srv_; - rclcpp::Subscription::SharedPtr joint_state_sub_; - rclcpp::Service::SharedPtr sync_srv; - rclcpp::Service::SharedPtr execute_traj_srv; - - std::vector joint_angle; - std::string collision_object_frame; - std::string base_frame_id; - std::string end_effector_frame_id; - std::string move_group_name; -}; - -#endif \ No newline at end of file diff --git a/ras_moveit/include/ras_moveit/moveit_server.hpp b/ras_moveit/include/ras_moveit/moveit_server.hpp new file mode 100644 index 0000000..15ef14f --- /dev/null +++ b/ras_moveit/include/ras_moveit/moveit_server.hpp @@ -0,0 +1,119 @@ +/* + * + * Copyright (C) 2024 Harsh Davda + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published + * by the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * For inquiries or further information, you may contact: + * Harsh Davda + * Email: info@opensciencestack.org + */ + +#ifndef MOVEIT_SERVER_HPP +#define MOVEIT_SERVER_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using moveit::planning_interface::MoveGroupInterface; + + +struct OrientationTolerance +{ + double x; + double y; + double z; +}; + +struct PlanningParameters +{ + OrientationTolerance orientation_tolerance; + int planning_attempts; + double planning_time; + double goal_tolerance; + double goal_orientation_tolerance; + double velocity_scaling_factor; + double acceleration_scaling_factor; +}; + +class MoveitServer : public rclcpp::Node, public std::enable_shared_from_this +{ +public: + MoveitServer(std::shared_ptr move_group_node); + ~MoveitServer(); + void move_to_pose_callback(const std::shared_ptr request, std::shared_ptr response); + void move_to_joint_callback(const std::shared_ptr request, std::shared_ptr response); + void rotate_effector_callback(const std::shared_ptr request, std::shared_ptr response); + void joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr msg); + void sync_callback(const std::shared_ptr request, std::shared_ptr response); + void set_constraints(const geometry_msgs::msg::Pose::_orientation_type &quat); + bool Execute(geometry_msgs::msg::Pose target_pose); + bool Execute(sensor_msgs::msg::JointState target_joints); + void trajectory_callback(const std::shared_ptr request, + std::shared_ptr response); + // void trajectory_callback(const std::shared_ptr request, + // std::shared_ptr response); + void AddScenePlane(); + +private: + void configure_move_group(); + bool plan_and_execute_with_retries(); + + std::shared_ptr move_group_arm; + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + // moveit::core::RobotStatePtr current_state_arm; + // const moveit::core::JointModelGroup *joint_model_group_arm; + // moveit::planning_interface::MoveGroupInterface::Plan my_plan_arm; + + rclcpp::Service::SharedPtr move_to_pose_srv_; + rclcpp::Service::SharedPtr move_to_joint_srv_; + rclcpp::Publisher::SharedPtr trajectory_pub; + rclcpp::Service::SharedPtr rotate_effector_srv_; + rclcpp::Subscription::SharedPtr joint_state_sub_; + rclcpp::Service::SharedPtr sync_srv; + rclcpp::Service::SharedPtr execute_traj_srv; + + std::vector joint_angle; + std::string collision_object_frame; + std::string base_frame_id; + std::string end_effector_frame_id; + std::string move_group_name; + + PlanningParameters planning_parameters_; +}; + +#endif \ No newline at end of file diff --git a/ras_moveit/launch/moveit_real_server.launch.py b/ras_moveit/launch/moveit_real_server.launch.py index 1622871..e9fe012 100644 --- a/ras_moveit/launch/moveit_real_server.launch.py +++ b/ras_moveit/launch/moveit_real_server.launch.py @@ -3,14 +3,18 @@ from ras_common.config.loaders.lab_setup import LabSetup as LabLoader from ras_resource_lib.managers.asset_manager import AssetManager,AssetType from ras_resource_lib.types.manipulator.component import ManipulatorComponent - +from ament_index_python.packages import get_package_share_directory +import os def generate_launch_description(): - AssetManager.init() - LabLoader.init() - robot_component : ManipulatorComponent = AssetManager.get_asset_component(LabLoader.robot_name,AssetType.MANIPULATOR) - return LaunchDescription([ - Node( + AssetManager.init() + LabLoader.init() + robot_component : ManipulatorComponent = AssetManager.get_asset_component(LabLoader.robot_name,AssetType.MANIPULATOR) + config_directory = os.path.join(get_package_share_directory('ras_moveit'), 'config') + params_file = os.path.join(config_directory, 'tune_params.yaml') + + return LaunchDescription([ + Node( package='ras_moveit', # namespace='moveit_server', executable='moveit_server', @@ -18,7 +22,7 @@ def generate_launch_description(): output='screen', parameters=[ {'move_group_name': robot_component.movegroup_name}, - ] - ), + params_file + ]), - ]) \ No newline at end of file + ]) \ No newline at end of file diff --git a/ras_moveit/launch/moveit_server.launch.py b/ras_moveit/launch/moveit_server.launch.py index 1622871..451c377 100644 --- a/ras_moveit/launch/moveit_server.launch.py +++ b/ras_moveit/launch/moveit_server.launch.py @@ -3,14 +3,19 @@ from ras_common.config.loaders.lab_setup import LabSetup as LabLoader from ras_resource_lib.managers.asset_manager import AssetManager,AssetType from ras_resource_lib.types.manipulator.component import ManipulatorComponent +from ament_index_python.packages import get_package_share_directory +import os def generate_launch_description(): - AssetManager.init() - LabLoader.init() - robot_component : ManipulatorComponent = AssetManager.get_asset_component(LabLoader.robot_name,AssetType.MANIPULATOR) - return LaunchDescription([ - Node( + AssetManager.init() + LabLoader.init() + robot_component : ManipulatorComponent = AssetManager.get_asset_component(LabLoader.robot_name,AssetType.MANIPULATOR) + config_directory = os.path.join(get_package_share_directory('ras_moveit'), 'config') + params_file = os.path.join(config_directory, 'tune_params.yaml') + + return LaunchDescription([ + Node( package='ras_moveit', # namespace='moveit_server', executable='moveit_server', @@ -18,7 +23,6 @@ def generate_launch_description(): output='screen', parameters=[ {'move_group_name': robot_component.movegroup_name}, - ] - ), - - ]) \ No newline at end of file + params_file + ]), + ]) \ No newline at end of file diff --git a/ras_moveit/src/moveit_server.cpp b/ras_moveit/src/moveit_server.cpp index f6c07fd..80efc25 100644 --- a/ras_moveit/src/moveit_server.cpp +++ b/ras_moveit/src/moveit_server.cpp @@ -1,434 +1,226 @@ /* - * + * * Copyright (C) 2024 Harsh Davda - * + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU Affero General Public License as published * by the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. - * + * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU Affero General Public License for more details. - * + * * You should have received a copy of the GNU Affero General Public License * along with this program. If not, see . - * + * * For inquiries or further information, you may contact: * Harsh Davda * Email: info@opensciencestack.org -*/ - -#include "../include/moveit_server.hpp" - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("Moveit Server Init"); + */ +#include "ras_moveit/moveit_server.hpp" +// static const rclcpp::this->get_logger() this->get_logger() = rclcpp::get_logger("Moveit Server Init"); MoveitServer::MoveitServer(std::shared_ptr move_group_node) : Node("moveit_server") - { - - this->declare_parameter("move_group_name", "lite6"); - this->declare_parameter("collision_object_frame", "world"); - this->declare_parameter("base_frame_id", "link_base"); - this->declare_parameter("end_effector_frame_id", "link_eef"); - - move_group_name = this->get_parameter("move_group_name").as_string(); - collision_object_frame = this->get_parameter("collision_object_frame").as_string(); - base_frame_id = this->get_parameter("base_frame_id").as_string(); - end_effector_frame_id = this->get_parameter("end_effector_frame_id").as_string(); - - move_group_arm = std::make_shared( - move_group_node, - move_group_name - ); - - - RCLCPP_INFO(LOGGER, "Node Started"); - - move_to_pose_srv_ = this->create_service( - "/create_traj", - std::bind(&MoveitServer::move_to_pose_callback, this, std::placeholders::_1, std::placeholders::_2)); - - move_to_joint_srv_ = this->create_service( - "/move_to_joint_states", - std::bind(&MoveitServer::move_to_joint_callback, this, std::placeholders::_1, std::placeholders::_2)); - - rotate_effector_srv_ = this->create_service( - "/rotate_effector", - std::bind(&MoveitServer::rotate_effector_callback, this, std::placeholders::_1, std::placeholders::_2)); - - joint_state_sub_ = this->create_subscription( - "/joint_states", 10, std::bind(&MoveitServer::joint_state_callback, this, std::placeholders::_1)); - - trajectory_pub = this->create_publisher("trajectory_topic", 10); - - sync_srv = this->create_service( - "/sync_arm", - std::bind(&MoveitServer::sync_callback, this, std::placeholders::_1, std::placeholders::_2) - ); - - execute_traj_srv = this->create_service( - "trajectory_topic", - std::bind(&MoveitServer::trajectory_callback, this, std::placeholders::_1, std::placeholders::_2)); - - AddScenePlane(); - } +{ + + + this->declare_parameter("move_group_name", "lite6"); + this->declare_parameter("collision_object_frame", "world"); + this->declare_parameter("base_frame_id", "link_base"); + this->declare_parameter("end_effector_frame_id", "link_eef"); + this->declare_parameter("orientation_tolerance.x", 0.75); + this->declare_parameter("orientation_tolerance.y", 0.75); + this->declare_parameter("orientation_tolerance.z", 0.75); + this->declare_parameter("planning_attempts", 5); + this->declare_parameter("planning_time", 2.0); + this->declare_parameter("goal_tolerance", 0.005); + this->declare_parameter("goal_orientation_tolerance", 0.005); + this->declare_parameter("velocity_scaling_factor", 0.2); + this->declare_parameter("acceleration_scaling_factor", 0.4); + + planning_parameters_.orientation_tolerance.x = this->get_parameter("orientation_tolerance.x").as_double(); + planning_parameters_.orientation_tolerance.y = this->get_parameter("orientation_tolerance.y").as_double(); + planning_parameters_.orientation_tolerance.z = this->get_parameter("orientation_tolerance.z").as_double(); + planning_parameters_.planning_attempts = this->get_parameter("planning_attempts").as_int(); + planning_parameters_.planning_time = this->get_parameter("planning_time").as_double(); + planning_parameters_.goal_tolerance = this->get_parameter("goal_tolerance").as_double(); + planning_parameters_.goal_orientation_tolerance = this->get_parameter("goal_orientation_tolerance").as_double(); + planning_parameters_.velocity_scaling_factor = this->get_parameter("velocity_scaling_factor").as_double(); + planning_parameters_.acceleration_scaling_factor = this->get_parameter("acceleration_scaling_factor").as_double(); - void MoveitServer::trajectory_callback(const std::shared_ptr request, - std::shared_ptr response) - { - RCLCPP_INFO(this->get_logger(), "Received trajectory message"); - - moveit_msgs::msg::RobotTrajectory robot_trajectory; - - // Convert JointTrajectory to RobotTrajectory - robot_trajectory.joint_trajectory = request->traj; - - bool success = (move_group_arm->execute(robot_trajectory) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - - response->success = 1; - } - void MoveitServer::AddScenePlane() - { - moveit_msgs::msg::CollisionObject collision_object; - collision_object.header.frame_id = collision_object_frame; - collision_object.id = "ground_plane"; - shape_msgs::msg::SolidPrimitive primitive; - primitive.type = primitive.BOX; - primitive.dimensions.resize(3); - primitive.dimensions[primitive.BOX_X] = 10; - primitive.dimensions[primitive.BOX_Y] = 10; - primitive.dimensions[primitive.BOX_Z] = 0.01; - geometry_msgs::msg::Pose box_pose; - box_pose.orientation.w = 1.0; - box_pose.position.x = 0.0; - box_pose.position.y = 0.0; - box_pose.position.z = -0.05; - collision_object.primitives.push_back(primitive); - collision_object.primitive_poses.push_back(box_pose); - collision_object.operation = collision_object.ADD; - planning_scene_interface.applyCollisionObject(collision_object); - RCLCPP_INFO(this->get_logger(), "Added plane to planning scene"); - } - - void MoveitServer::set_constraints(const geometry_msgs::msg::Pose::_orientation_type& quat) - { - RCLCPP_INFO(LOGGER, "Orientation Constrains Set"); + move_group_name = this->get_parameter("move_group_name").as_string(); + RCLCPP_INFO(this->get_logger(), "Movegrup name is %s Acceleration scaling factor: %f", move_group_name.c_str(), planning_parameters_.acceleration_scaling_factor); + collision_object_frame = this->get_parameter("collision_object_frame").as_string(); + base_frame_id = this->get_parameter("base_frame_id").as_string(); + end_effector_frame_id = this->get_parameter("end_effector_frame_id").as_string(); - // Goal constraints - position + move_group_arm = std::make_shared( + move_group_node, + move_group_name); + RCLCPP_INFO(this->get_logger(), "Node Started"); - moveit_msgs::msg::Constraints goal_constraints; + move_to_pose_srv_ = this->create_service( + "/create_traj", + std::bind(&MoveitServer::move_to_pose_callback, this, std::placeholders::_1, std::placeholders::_2)); - // Goal constraints - orientation - moveit_msgs::msg::OrientationConstraint ori_constraint; - ori_constraint.header.stamp = this->get_clock()->now(); // Set the current time - ori_constraint.header.frame_id = base_frame_id; - ori_constraint.orientation.x = quat.x; - ori_constraint.orientation.y = quat.y; - ori_constraint.orientation.z = quat.z; - ori_constraint.orientation.w = quat.w; - ori_constraint.link_name = end_effector_frame_id; - ori_constraint.absolute_x_axis_tolerance = 0.75; - ori_constraint.absolute_y_axis_tolerance = 0.75; - ori_constraint.absolute_z_axis_tolerance = 0.75; - ori_constraint.weight = 1.0; - ori_constraint.parameterization = 1.0; - goal_constraints.orientation_constraints.push_back(ori_constraint); - - move_group_arm->setPathConstraints(goal_constraints); - } + move_to_joint_srv_ = this->create_service( + "/move_to_joint_states", + std::bind(&MoveitServer::move_to_joint_callback, this, std::placeholders::_1, std::placeholders::_2)); + rotate_effector_srv_ = this->create_service( + "/rotate_effector", + std::bind(&MoveitServer::rotate_effector_callback, this, std::placeholders::_1, std::placeholders::_2)); - bool MoveitServer::Execute(sensor_msgs::msg::JointState target_joints) - { - RCLCPP_INFO(this->get_logger(),"function call"); + joint_state_sub_ = this->create_subscription( + "/joint_states", 10, std::bind(&MoveitServer::joint_state_callback, this, std::placeholders::_1)); - trajectory_msgs::msg::JointTrajectory trajectory_msg; - - // move_group_arm->clearPathConstraints(); - RCLCPP_INFO(this->get_logger(),"clear constraints"); - - move_group_arm->setWorkspace(-1.0, -1.0, -1.0, 1.0, 1.0, 1.0); - - move_group_arm->setPlannerId("RRTConnectkConfigDefault"); - - move_group_arm->setNumPlanningAttempts(5); - move_group_arm->setPlanningTime(1); - move_group_arm->setGoalTolerance(0.005); - move_group_arm->setGoalOrientationTolerance(0.005); - move_group_arm->setMaxVelocityScalingFactor(0.2); - move_group_arm->setMaxAccelerationScalingFactor(0.4); - RCLCPP_INFO(this->get_logger(),"beforeconstraints"); - - - RCLCPP_INFO(this->get_logger(),"after constraints"); - move_group_arm->setJointValueTarget(target_joints); - - RCLCPP_INFO(this->get_logger(),"after target pose"); - - int count = 15; - for (int i = 0; i < count; i++) - { - if (i < count-2) - { - moveit::planning_interface::MoveGroupInterface::Plan my_plan; - bool success = (move_group_arm->plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - RCLCPP_INFO(this->get_logger(),"after target plan"); - trajectory_msg = my_plan.trajectory_.joint_trajectory; - if (success) - { - move_group_arm->execute(my_plan); - trajectory_pub->publish(trajectory_msg); - return 1; - } - } - else - { - move_group_arm->clearPathConstraints(); - RCLCPP_INFO(this->get_logger(), "Clearning Constraints"); - moveit::planning_interface::MoveGroupInterface::Plan my_plan2; - bool success2 = (move_group_arm->plan(my_plan2) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - trajectory_msg = my_plan2.trajectory_.joint_trajectory; - if (success2) - { - move_group_arm->execute(my_plan2); - trajectory_pub->publish(trajectory_msg); - return 1; - } - else - { - return 0; - } - } - } - } - - bool MoveitServer::Execute(geometry_msgs::msg::Pose target_pose) { - RCLCPP_INFO(this->get_logger(),"function call"); - - trajectory_msgs::msg::JointTrajectory trajectory_msg; - - // move_group_arm->clearPathConstraints(); - RCLCPP_INFO(this->get_logger(),"clear constraints"); - - move_group_arm->setWorkspace(-1.0, -1.0, -1.0, 1.0, 1.0, 1.0); - - move_group_arm->setPlannerId("RRTConnectkConfigDefault"); - - move_group_arm->setNumPlanningAttempts(5); - move_group_arm->setPlanningTime(1); - move_group_arm->setGoalTolerance(0.005); - move_group_arm->setGoalOrientationTolerance(0.005); - move_group_arm->setMaxVelocityScalingFactor(0.2); - move_group_arm->setMaxAccelerationScalingFactor(0.4); - RCLCPP_INFO(this->get_logger(),"beforeconstraints"); - - set_constraints(target_pose.orientation); - - RCLCPP_INFO(this->get_logger(),"after constraints"); - move_group_arm->setPoseTarget(target_pose); - - RCLCPP_INFO(this->get_logger(),"after target pose"); - - int count = 15; - for (int i = 0; i < count; i++) - { - if (i < count-2) - { - moveit::planning_interface::MoveGroupInterface::Plan my_plan; - bool success = (move_group_arm->plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - RCLCPP_INFO(this->get_logger(),"after target plan"); - trajectory_msg = my_plan.trajectory_.joint_trajectory; - if (success) - { - move_group_arm->execute(my_plan); - trajectory_pub->publish(trajectory_msg); - return 1; - } - } - else - { - move_group_arm->clearPathConstraints(); - RCLCPP_INFO(this->get_logger(), "Clearning Constraints"); - moveit::planning_interface::MoveGroupInterface::Plan my_plan2; - bool success2 = (move_group_arm->plan(my_plan2) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - trajectory_msg = my_plan2.trajectory_.joint_trajectory; - if (success2) - { - move_group_arm->execute(my_plan2); - trajectory_pub->publish(trajectory_msg); - return 1; - } - else - { - return 0; - } - } - } - } + trajectory_pub = this->create_publisher("trajectory_topic", 10); - void MoveitServer::move_to_pose_callback( - const std::shared_ptr request, - std::shared_ptr response) - { - RCLCPP_WARN(this->get_logger(), "Received Pose request..."); + sync_srv = this->create_service( + "/sync_arm", + std::bind(&MoveitServer::sync_callback, this, std::placeholders::_1, std::placeholders::_2)); - geometry_msgs::msg::Pose config_pose; - config_pose = request->object_pose; + execute_traj_srv = this->create_service( + "trajectory_topic", + std::bind(&MoveitServer::trajectory_callback, this, std::placeholders::_1, std::placeholders::_2)); - RCLCPP_INFO(this->get_logger(), - "Received pose:\n" - "Position: x=%f, y=%f, z=%f\n" - "Orientation: x=%f, y=%f, z=%f, w=%f", - config_pose.position.x, config_pose.position.y, config_pose.position.z, - config_pose.orientation.x, config_pose.orientation.y, config_pose.orientation.z, config_pose.orientation.w - ); + AddScenePlane(); +} - bool status = Execute(config_pose); - - response->success = status; - } +MoveitServer::~MoveitServer() +{ +} - void MoveitServer::move_to_joint_callback( - const std::shared_ptr request, - std::shared_ptr response) - { - RCLCPP_WARN(this->get_logger(), "Received Joint request..."); +void MoveitServer::AddScenePlane() +{ + moveit_msgs::msg::CollisionObject collision_object; + collision_object.header.frame_id = collision_object_frame; + collision_object.id = "ground_plane"; + shape_msgs::msg::SolidPrimitive primitive; + primitive.type = primitive.BOX; + primitive.dimensions.resize(3); + primitive.dimensions[primitive.BOX_X] = 10; + primitive.dimensions[primitive.BOX_Y] = 10; + primitive.dimensions[primitive.BOX_Z] = 0.01; + geometry_msgs::msg::Pose box_pose; + box_pose.orientation.w = 1.0; + box_pose.position.x = 0.0; + box_pose.position.y = 0.0; + box_pose.position.z = -0.05; + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + collision_object.operation = collision_object.ADD; + planning_scene_interface.applyCollisionObject(collision_object); + RCLCPP_INFO(this->get_logger(), "Added plane to planning scene"); +} - sensor_msgs::msg::JointState config_joints; - config_joints = request->joints; +void MoveitServer::set_constraints(const geometry_msgs::msg::Pose::_orientation_type &quat) +{ + RCLCPP_INFO(this->get_logger(), "Orientation Constrains Set"); + + // Goal constraints - position + moveit_msgs::msg::Constraints goal_constraints; + + // Goal constraints - orientation + moveit_msgs::msg::OrientationConstraint ori_constraint; + ori_constraint.header.stamp = this->get_clock()->now(); + ori_constraint.header.frame_id = base_frame_id; + ori_constraint.orientation.x = quat.x; + ori_constraint.orientation.y = quat.y; + ori_constraint.orientation.z = quat.z; + ori_constraint.orientation.w = quat.w; + ori_constraint.link_name = end_effector_frame_id; + ori_constraint.absolute_x_axis_tolerance = planning_parameters_.orientation_tolerance.x; + ori_constraint.absolute_y_axis_tolerance = planning_parameters_.orientation_tolerance.y; + ori_constraint.absolute_z_axis_tolerance = planning_parameters_.orientation_tolerance.z; + ori_constraint.weight = 1.0; + ori_constraint.parameterization = 1.0; + goal_constraints.orientation_constraints.push_back(ori_constraint); + + move_group_arm->setPathConstraints(goal_constraints); +} +bool MoveitServer::Execute(sensor_msgs::msg::JointState target_joints) +{ + // move_group_arm->clearPathConstraints(); + // RCLCPP_INFO(this->get_logger(), "clear constraints"); + configure_move_group(); + move_group_arm->setJointValueTarget(target_joints); + return plan_and_execute_with_retries(); +} - std::ostringstream joint_info; - joint_info << "Received joints:\n"; - for (size_t i = 0; i < config_joints.position.size(); ++i) { - joint_info << "Joint" << (i + 1) << ": " << config_joints.position[i] << "\n"; - } - RCLCPP_INFO(this->get_logger(), joint_info.str().c_str()); +bool MoveitServer::Execute(geometry_msgs::msg::Pose target_pose) +{ + trajectory_msgs::msg::JointTrajectory trajectory_msg; + // move_group_arm->clearPathConstraints(); + RCLCPP_INFO(this->get_logger(), "clear constraints"); + + configure_move_group(); + set_constraints(target_pose.orientation); + move_group_arm->setPoseTarget(target_pose); + return plan_and_execute_with_retries(); +} - bool status = Execute(config_joints); - - response->success = status; - } +void MoveitServer::configure_move_group() +{ + move_group_arm->setWorkspace(-1.0, -1.0, -1.0, 1.0, 1.0, 1.0); + move_group_arm->setPlannerId("RRTConnectkConfigDefault"); + move_group_arm->setNumPlanningAttempts(planning_parameters_.planning_attempts); + move_group_arm->setPlanningTime(planning_parameters_.planning_time); + move_group_arm->setGoalTolerance(planning_parameters_.goal_tolerance); + move_group_arm->setGoalOrientationTolerance(planning_parameters_.goal_orientation_tolerance); + move_group_arm->setMaxVelocityScalingFactor(planning_parameters_.velocity_scaling_factor); + move_group_arm->setMaxAccelerationScalingFactor(planning_parameters_.acceleration_scaling_factor); +} - void MoveitServer::rotate_effector_callback( - const std::shared_ptr request, - std::shared_ptr response) +bool MoveitServer::plan_and_execute_with_retries() +{ + trajectory_msgs::msg::JointTrajectory trajectory_msg; + int count = 15; + for (int i = 0; i < count; i++) { - trajectory_msgs::msg::JointTrajectory trajectory_msg; - RCLCPP_INFO(LOGGER, "Received RotateEffector request to rotate end effector by angle: %f", request->rotation_angle); - - - move_group_arm->setWorkspace(-1.0, -1.0, -1.0, 1.0, 1.0, 1.0); - - move_group_arm->setPlannerId("RRTConnectkConfigDefault"); - - move_group_arm->setNumPlanningAttempts(5); - move_group_arm->setPlanningTime(2); - move_group_arm->setGoalTolerance(0.005); - move_group_arm->setGoalOrientationTolerance(0.005); - move_group_arm->setMaxVelocityScalingFactor(0.4); - move_group_arm->setMaxAccelerationScalingFactor(0.4); - move_group_arm->clearPathConstraints(); - - // Ensure joint states are available - if (joint_angle.empty()) + if (i < count - 2) { - RCLCPP_ERROR(LOGGER, "No joint states available. Aborting rotation."); - response->success = false; - return; - } - - int count = 5; - - // set_constraints(); - // Convert joint_angle (std::vector) to std::vector - std::vector target_joint_values = {joint_angle[2], joint_angle[0], joint_angle[1], joint_angle[3], joint_angle[4], joint_angle[5]+(request->rotation_angle)}; - - // Update the last joint (assumed to be joint6) by adding the rotation - - // target_joint_values[5] += request->rotation_angle; // Add rotation in radians to joint6 - RCLCPP_INFO(LOGGER, "Updated joint6 angle: %f", target_joint_values[5]); - - - for (int i = 0; i < count; i++) + moveit::planning_interface::MoveGroupInterface::Plan my_plan; + bool success = (move_group_arm->plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + trajectory_msg = my_plan.trajectory_.joint_trajectory; + if (success) { - move_group_arm->setJointValueTarget(target_joint_values); - moveit::planning_interface::MoveGroupInterface::Plan my_plan3; - bool success = (move_group_arm->plan(my_plan3) == moveit::core::MoveItErrorCode::SUCCESS); - trajectory_msg = my_plan3.trajectory_.joint_trajectory; - - if (success) - { - move_group_arm->execute(my_plan3); + move_group_arm->execute(my_plan); trajectory_pub->publish(trajectory_msg); - response->success = true; - RCLCPP_INFO(LOGGER, "End effector rotation executed successfully."); break; - } } - } - - void MoveitServer::sync_callback(const std::shared_ptr request, - std::shared_ptr response) - { - RCLCPP_INFO(this->get_logger(), "MoveitServer::sync_callback called"); - std::vector joint_values; - for (const auto& x : request->joint_state.position) - { - joint_values.push_back(x); } - - move_group_arm->setJointValueTarget(joint_values); - move_group_arm->setMaxVelocityScalingFactor(0.7); - move_group_arm->setMaxAccelerationScalingFactor(0.7); - // testing purpose - move_group_arm->setWorkspace(-1.0, -1.0, -1.0, 1.0, 1.0, 1.0); - - // move_group_arm->setPlannerId("RRTConnectkConfigDefault"); - - move_group_arm->setNumPlanningAttempts(5); - move_group_arm->setPlanningTime(2); - move_group_arm->setGoalTolerance(0.01); - move_group_arm->setGoalOrientationTolerance(0.035); - move_group_arm->clearPathConstraints(); - - moveit::planning_interface::MoveGroupInterface::Plan my_plan; - bool result = (move_group_arm->plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); - if (result) + else { - result = (move_group_arm->execute(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - if (result) + move_group_arm->clearPathConstraints(); + RCLCPP_INFO(this->get_logger(), "Clearning Constraints"); + moveit::planning_interface::MoveGroupInterface::Plan my_plan2; + bool success2 = (move_group_arm->plan(my_plan2) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + trajectory_msg = my_plan2.trajectory_.joint_trajectory; + if (success2) { - RCLCPP_INFO(this->get_logger(), "Trajectory executed successfully"); - response->successq = true; + move_group_arm->execute(my_plan2); + trajectory_pub->publish(trajectory_msg); + break; } else { - RCLCPP_ERROR(this->get_logger(), "Trajectory not executed"); - response->successq = false; + return 0; } } - else - { - RCLCPP_WARN(this->get_logger(), "Planning for trajectory failed"); - response->successq = false; - } - } - - void MoveitServer::joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr msg) // <-- Updated this line - { - joint_angle.clear(); - for (auto i : msg->position) - { - joint_angle.push_back(i); - } } + // TODO(Sachin): check if the goal executed successfully + return 1; +} -int main(int argc, char **argv) { +int main(int argc, char **argv) +{ rclcpp::init(argc, argv); auto move_group_node = rclcpp::Node::make_shared("moveit_server"); diff --git a/ras_moveit/src/moveit_services.cpp b/ras_moveit/src/moveit_services.cpp new file mode 100644 index 0000000..0663c2f --- /dev/null +++ b/ras_moveit/src/moveit_services.cpp @@ -0,0 +1,193 @@ +/* + * + * Copyright (C) 2025 Sachin Kumar + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published + * by the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * For inquiries or further information, you may contact: + * Sachin Kumar + * Email: info@opensciencestack.org + */ + +#include "ras_moveit/moveit_server.hpp" + +void MoveitServer::trajectory_callback(const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_INFO(this->get_logger(), "Received trajectory message"); + + moveit_msgs::msg::RobotTrajectory robot_trajectory; + + // Convert JointTrajectory to RobotTrajectory + robot_trajectory.joint_trajectory = request->traj; + + bool success = (move_group_arm->execute(robot_trajectory) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + + response->success = 1; +} + +void MoveitServer::move_to_pose_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_WARN(this->get_logger(), "Received Pose request..."); + + geometry_msgs::msg::Pose config_pose; + config_pose = request->object_pose; + + RCLCPP_INFO(this->get_logger(), + "Received pose:\n" + "Position: x=%f, y=%f, z=%f\n" + "Orientation: x=%f, y=%f, z=%f, w=%f", + config_pose.position.x, config_pose.position.y, config_pose.position.z, + config_pose.orientation.x, config_pose.orientation.y, config_pose.orientation.z, config_pose.orientation.w); + + bool status = Execute(config_pose); + + response->success = status; +} + +void MoveitServer::move_to_joint_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_WARN(this->get_logger(), "Received Joint request..."); + + sensor_msgs::msg::JointState config_joints; + config_joints = request->joints; + + std::ostringstream joint_info; + joint_info << "Received joints:\n"; + for (size_t i = 0; i < config_joints.position.size(); ++i) + { + joint_info << "Joint" << (i + 1) << ": " << config_joints.position[i] << "\n"; + } + RCLCPP_INFO(this->get_logger(), joint_info.str().c_str()); + + bool status = Execute(config_joints); + + response->success = status; +} + +void MoveitServer::rotate_effector_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + trajectory_msgs::msg::JointTrajectory trajectory_msg; + RCLCPP_INFO(this->get_logger(), "Received RotateEffector request to rotate end effector by angle: %f", request->rotation_angle); + + move_group_arm->setWorkspace(-1.0, -1.0, -1.0, 1.0, 1.0, 1.0); + + move_group_arm->setPlannerId("RRTConnectkConfigDefault"); + + move_group_arm->setNumPlanningAttempts(5); + move_group_arm->setPlanningTime(2); + move_group_arm->setGoalTolerance(0.005); + move_group_arm->setGoalOrientationTolerance(0.005); + move_group_arm->setMaxVelocityScalingFactor(0.4); + move_group_arm->setMaxAccelerationScalingFactor(0.4); + move_group_arm->clearPathConstraints(); + + // Ensure joint states are available + if (joint_angle.empty()) + { + RCLCPP_ERROR(this->get_logger(), "No joint states available. Aborting rotation."); + response->success = false; + return; + } + + int count = 5; + + // set_constraints(); + // Convert joint_angle (std::vector) to std::vector + std::vector target_joint_values = {joint_angle[2], joint_angle[0], joint_angle[1], joint_angle[3], joint_angle[4], joint_angle[5] + (request->rotation_angle)}; + + // Update the last joint (assumed to be joint6) by adding the rotation + + // target_joint_values[5] += request->rotation_angle; // Add rotation in radians to joint6 + RCLCPP_INFO(this->get_logger(), "Updated joint6 angle: %f", target_joint_values[5]); + + for (int i = 0; i < count; i++) + { + move_group_arm->setJointValueTarget(target_joint_values); + moveit::planning_interface::MoveGroupInterface::Plan my_plan3; + bool success = (move_group_arm->plan(my_plan3) == moveit::core::MoveItErrorCode::SUCCESS); + trajectory_msg = my_plan3.trajectory_.joint_trajectory; + + if (success) + { + move_group_arm->execute(my_plan3); + trajectory_pub->publish(trajectory_msg); + response->success = true; + RCLCPP_INFO(this->get_logger(), "End effector rotation executed successfully."); + break; + } + } +} + +void MoveitServer::sync_callback(const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_INFO(this->get_logger(), "MoveitServer::sync_callback called"); + std::vector joint_values; + for (const auto &x : request->joint_state.position) + { + joint_values.push_back(x); + } + + move_group_arm->setJointValueTarget(joint_values); + move_group_arm->setMaxVelocityScalingFactor(0.7); + move_group_arm->setMaxAccelerationScalingFactor(0.7); + // testing purpose + move_group_arm->setWorkspace(-1.0, -1.0, -1.0, 1.0, 1.0, 1.0); + + // move_group_arm->setPlannerId("RRTConnectkConfigDefault"); + + move_group_arm->setNumPlanningAttempts(5); + move_group_arm->setPlanningTime(2); + move_group_arm->setGoalTolerance(0.01); + move_group_arm->setGoalOrientationTolerance(0.035); + move_group_arm->clearPathConstraints(); + + moveit::planning_interface::MoveGroupInterface::Plan my_plan; + bool result = (move_group_arm->plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); + if (result) + { + result = (move_group_arm->execute(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + if (result) + { + RCLCPP_INFO(this->get_logger(), "Trajectory executed successfully"); + response->successq = true; + } + else + { + RCLCPP_ERROR(this->get_logger(), "Trajectory not executed"); + response->successq = false; + } + } + else + { + RCLCPP_WARN(this->get_logger(), "Planning for trajectory failed"); + response->successq = false; + } +} + +void MoveitServer::joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr msg) // <-- Updated this line +{ + joint_angle.clear(); + for (auto i : msg->position) + { + joint_angle.push_back(i); + } +} \ No newline at end of file From ce74fab8d649ebb023aaf43ceecd1440cb9f691c Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Fri, 14 Feb 2025 14:33:58 +0100 Subject: [PATCH 2/3] use case params added for trajectory and sync --- ras_moveit/config/tune_params.yaml | 27 ++++-- .../include/ras_moveit/moveit_server.hpp | 35 ++++++-- ras_moveit/src/moveit_server.cpp | 82 +++++++++++------ ras_moveit/src/moveit_services.cpp | 89 ++----------------- 4 files changed, 109 insertions(+), 124 deletions(-) diff --git a/ras_moveit/config/tune_params.yaml b/ras_moveit/config/tune_params.yaml index dfaffae..9796a23 100644 --- a/ras_moveit/config/tune_params.yaml +++ b/ras_moveit/config/tune_params.yaml @@ -7,9 +7,24 @@ moveit_server: x: 0.75 y: 0.75 z: 0.75 - planning_attempts: 5 - planning_time: 2.0 - goal_tolerance: 0.005 - goal_orientation_tolerance: 0.005 - velocity_scaling_factor: 0.2 - acceleration_scaling_factor: 0.4 + trajectory: + velocity_scaling_factor: 0.2 + acceleration_scaling_factor: 0.4 + planning_attempts: 5 + planning_time: 2.0 + goal_tolerance: 0.005 + goal_orientation_tolerance: 0.005 + sync: + velocity_scaling_factor: 0.8 + acceleration_scaling_factor: 1.0 + planning_attempts: 5 + planning_time: 2.0 + goal_tolerance: 0.005 + goal_orientation_tolerance: 0.005 + workspace: + minx: -1.0 + miny: -1.0 + minz: -1.0 + maxx: 1.0 + maxy: 1.0 + maxz: 1.0 diff --git a/ras_moveit/include/ras_moveit/moveit_server.hpp b/ras_moveit/include/ras_moveit/moveit_server.hpp index 15ef14f..a755ede 100644 --- a/ras_moveit/include/ras_moveit/moveit_server.hpp +++ b/ras_moveit/include/ras_moveit/moveit_server.hpp @@ -52,22 +52,39 @@ using moveit::planning_interface::MoveGroupInterface; -struct OrientationTolerance +struct Tolerance { double x; double y; double z; }; -struct PlanningParameters +struct Workspace +{ + double minx; + double miny; + double minz; + double maxx; + double maxy; + double maxz; +}; + +struct MotionFactor { - OrientationTolerance orientation_tolerance; + double velocity_scaling_factor; + double acceleration_scaling_factor; int planning_attempts; double planning_time; double goal_tolerance; double goal_orientation_tolerance; - double velocity_scaling_factor; - double acceleration_scaling_factor; +}; + +struct PlanningParameters +{ + Tolerance orientation_tolerance; + Workspace workspace; + MotionFactor sync; + MotionFactor trajectory; }; class MoveitServer : public rclcpp::Node, public std::enable_shared_from_this @@ -81,8 +98,8 @@ class MoveitServer : public rclcpp::Node, public std::enable_shared_from_this request, std::shared_ptr response); void set_constraints(const geometry_msgs::msg::Pose::_orientation_type &quat); - bool Execute(geometry_msgs::msg::Pose target_pose); - bool Execute(sensor_msgs::msg::JointState target_joints); + bool Execute(const geometry_msgs::msg::Pose target_pose, const MotionFactor motion); + bool Execute(const sensor_msgs::msg::JointState target_joints, const MotionFactor motion); void trajectory_callback(const std::shared_ptr request, std::shared_ptr response); // void trajectory_callback(const std::shared_ptr request, @@ -90,8 +107,8 @@ class MoveitServer : public rclcpp::Node, public std::enable_shared_from_this move_group_arm; moveit::planning_interface::PlanningSceneInterface planning_scene_interface; diff --git a/ras_moveit/src/moveit_server.cpp b/ras_moveit/src/moveit_server.cpp index 80efc25..a19bdcc 100644 --- a/ras_moveit/src/moveit_server.cpp +++ b/ras_moveit/src/moveit_server.cpp @@ -34,26 +34,44 @@ MoveitServer::MoveitServer(std::shared_ptr move_group_node) this->declare_parameter("orientation_tolerance.x", 0.75); this->declare_parameter("orientation_tolerance.y", 0.75); this->declare_parameter("orientation_tolerance.z", 0.75); - this->declare_parameter("planning_attempts", 5); - this->declare_parameter("planning_time", 2.0); - this->declare_parameter("goal_tolerance", 0.005); - this->declare_parameter("goal_orientation_tolerance", 0.005); - this->declare_parameter("velocity_scaling_factor", 0.2); - this->declare_parameter("acceleration_scaling_factor", 0.4); + this->declare_parameter("trajectory.planning_attempts", 5); + this->declare_parameter("trajectory.planning_time", 2.0); + this->declare_parameter("trajectory.goal_tolerance", 0.005); + this->declare_parameter("trajectory.goal_orientation_tolerance", 0.005); + this->declare_parameter("trajectory.velocity_scaling_factor", 0.2); + this->declare_parameter("trajectory.acceleration_scaling_factor", 0.4); + this->declare_parameter("sync.planning_attempts", 5); + this->declare_parameter("sync.planning_time", 2.0); + this->declare_parameter("sync.goal_tolerance", 0.005); + this->declare_parameter("sync.goal_orientation_tolerance", 0.005); + this->declare_parameter("sync.velocity_scaling_factor", 0.2); + this->declare_parameter("sync.acceleration_scaling_factor", 0.4); + + this->declare_parameter("workspace.minx", -1.0); + this->declare_parameter("workspace.miny", -1.0); + this->declare_parameter("workspace.minz", -1.0); + this->declare_parameter("workspace.maxx", 1.0); + this->declare_parameter("workspace.maxy", 1.0); + this->declare_parameter("workspace.maxz", 1.0); planning_parameters_.orientation_tolerance.x = this->get_parameter("orientation_tolerance.x").as_double(); planning_parameters_.orientation_tolerance.y = this->get_parameter("orientation_tolerance.y").as_double(); planning_parameters_.orientation_tolerance.z = this->get_parameter("orientation_tolerance.z").as_double(); - planning_parameters_.planning_attempts = this->get_parameter("planning_attempts").as_int(); - planning_parameters_.planning_time = this->get_parameter("planning_time").as_double(); - planning_parameters_.goal_tolerance = this->get_parameter("goal_tolerance").as_double(); - planning_parameters_.goal_orientation_tolerance = this->get_parameter("goal_orientation_tolerance").as_double(); - planning_parameters_.velocity_scaling_factor = this->get_parameter("velocity_scaling_factor").as_double(); - planning_parameters_.acceleration_scaling_factor = this->get_parameter("acceleration_scaling_factor").as_double(); + planning_parameters_.trajectory.planning_attempts = this->get_parameter("trajectory.planning_attempts").as_int(); + planning_parameters_.trajectory.planning_time = this->get_parameter("trajectory.planning_time").as_double(); + planning_parameters_.trajectory.goal_tolerance = this->get_parameter("trajectory.goal_tolerance").as_double(); + planning_parameters_.trajectory.goal_orientation_tolerance = this->get_parameter("trajectory.goal_orientation_tolerance").as_double(); + planning_parameters_.trajectory.velocity_scaling_factor = this->get_parameter("trajectory.velocity_scaling_factor").as_double(); + planning_parameters_.trajectory.acceleration_scaling_factor = this->get_parameter("trajectory.acceleration_scaling_factor").as_double(); + planning_parameters_.sync.planning_attempts = this->get_parameter("sync.planning_attempts").as_int(); + planning_parameters_.sync.planning_time = this->get_parameter("sync.planning_time").as_double(); + planning_parameters_.sync.goal_tolerance = this->get_parameter("sync.goal_tolerance").as_double(); + planning_parameters_.sync.goal_orientation_tolerance = this->get_parameter("sync.goal_orientation_tolerance").as_double(); + planning_parameters_.sync.velocity_scaling_factor = this->get_parameter("sync.velocity_scaling_factor").as_double(); + planning_parameters_.sync.acceleration_scaling_factor = this->get_parameter("sync.acceleration_scaling_factor").as_double(); move_group_name = this->get_parameter("move_group_name").as_string(); - RCLCPP_INFO(this->get_logger(), "Movegrup name is %s Acceleration scaling factor: %f", move_group_name.c_str(), planning_parameters_.acceleration_scaling_factor); collision_object_frame = this->get_parameter("collision_object_frame").as_string(); base_frame_id = this->get_parameter("base_frame_id").as_string(); end_effector_frame_id = this->get_parameter("end_effector_frame_id").as_string(); @@ -145,40 +163,40 @@ void MoveitServer::set_constraints(const geometry_msgs::msg::Pose::_orientation_ move_group_arm->setPathConstraints(goal_constraints); } -bool MoveitServer::Execute(sensor_msgs::msg::JointState target_joints) +bool MoveitServer::Execute(const sensor_msgs::msg::JointState target_joints, const MotionFactor motion) { // move_group_arm->clearPathConstraints(); // RCLCPP_INFO(this->get_logger(), "clear constraints"); - configure_move_group(); + configure_move_group(motion); move_group_arm->setJointValueTarget(target_joints); return plan_and_execute_with_retries(); } -bool MoveitServer::Execute(geometry_msgs::msg::Pose target_pose) +bool MoveitServer::Execute(const geometry_msgs::msg::Pose target_pose, const MotionFactor motion) { trajectory_msgs::msg::JointTrajectory trajectory_msg; // move_group_arm->clearPathConstraints(); RCLCPP_INFO(this->get_logger(), "clear constraints"); - configure_move_group(); + configure_move_group(motion); set_constraints(target_pose.orientation); move_group_arm->setPoseTarget(target_pose); return plan_and_execute_with_retries(); } -void MoveitServer::configure_move_group() +void MoveitServer::configure_move_group(const MotionFactor motion_factor) { - move_group_arm->setWorkspace(-1.0, -1.0, -1.0, 1.0, 1.0, 1.0); - move_group_arm->setPlannerId("RRTConnectkConfigDefault"); - move_group_arm->setNumPlanningAttempts(planning_parameters_.planning_attempts); - move_group_arm->setPlanningTime(planning_parameters_.planning_time); - move_group_arm->setGoalTolerance(planning_parameters_.goal_tolerance); - move_group_arm->setGoalOrientationTolerance(planning_parameters_.goal_orientation_tolerance); - move_group_arm->setMaxVelocityScalingFactor(planning_parameters_.velocity_scaling_factor); - move_group_arm->setMaxAccelerationScalingFactor(planning_parameters_.acceleration_scaling_factor); + move_group_arm->setWorkspace(planning_parameters_.workspace.minx, planning_parameters_.workspace.miny, planning_parameters_.workspace.minz, planning_parameters_.workspace.maxx, planning_parameters_.workspace.maxy, planning_parameters_.workspace.maxz); + // move_group_arm->setPlannerId("RRTConnectkConfigDefault"); // TODO(Sachin): Check if this planner is defined already + move_group_arm->setNumPlanningAttempts(motion_factor.planning_attempts); + move_group_arm->setPlanningTime(motion_factor.planning_time); + move_group_arm->setGoalTolerance(motion_factor.goal_tolerance); + move_group_arm->setGoalOrientationTolerance(motion_factor.goal_orientation_tolerance); + move_group_arm->setMaxVelocityScalingFactor(motion_factor.velocity_scaling_factor); + move_group_arm->setMaxAccelerationScalingFactor(motion_factor.acceleration_scaling_factor); } -bool MoveitServer::plan_and_execute_with_retries() +bool MoveitServer::plan_and_execute_with_retries(bool publish_trajectory) { trajectory_msgs::msg::JointTrajectory trajectory_msg; int count = 15; @@ -192,7 +210,10 @@ bool MoveitServer::plan_and_execute_with_retries() if (success) { move_group_arm->execute(my_plan); - trajectory_pub->publish(trajectory_msg); + if (publish_trajectory) + { + trajectory_pub->publish(trajectory_msg); + } break; } } @@ -206,7 +227,10 @@ bool MoveitServer::plan_and_execute_with_retries() if (success2) { move_group_arm->execute(my_plan2); - trajectory_pub->publish(trajectory_msg); + if (publish_trajectory) + { + trajectory_pub->publish(trajectory_msg); + } break; } else diff --git a/ras_moveit/src/moveit_services.cpp b/ras_moveit/src/moveit_services.cpp index 0663c2f..231dd63 100644 --- a/ras_moveit/src/moveit_services.cpp +++ b/ras_moveit/src/moveit_services.cpp @@ -53,9 +53,7 @@ void MoveitServer::move_to_pose_callback( config_pose.position.x, config_pose.position.y, config_pose.position.z, config_pose.orientation.x, config_pose.orientation.y, config_pose.orientation.z, config_pose.orientation.w); - bool status = Execute(config_pose); - - response->success = status; + response->success = Execute(config_pose, planning_parameters_.trajectory); } void MoveitServer::move_to_joint_callback( @@ -75,30 +73,13 @@ void MoveitServer::move_to_joint_callback( } RCLCPP_INFO(this->get_logger(), joint_info.str().c_str()); - bool status = Execute(config_joints); - - response->success = status; + response->success = Execute(config_joints, planning_parameters_.trajectory); } void MoveitServer::rotate_effector_callback( const std::shared_ptr request, std::shared_ptr response) { - trajectory_msgs::msg::JointTrajectory trajectory_msg; - RCLCPP_INFO(this->get_logger(), "Received RotateEffector request to rotate end effector by angle: %f", request->rotation_angle); - - move_group_arm->setWorkspace(-1.0, -1.0, -1.0, 1.0, 1.0, 1.0); - - move_group_arm->setPlannerId("RRTConnectkConfigDefault"); - - move_group_arm->setNumPlanningAttempts(5); - move_group_arm->setPlanningTime(2); - move_group_arm->setGoalTolerance(0.005); - move_group_arm->setGoalOrientationTolerance(0.005); - move_group_arm->setMaxVelocityScalingFactor(0.4); - move_group_arm->setMaxAccelerationScalingFactor(0.4); - move_group_arm->clearPathConstraints(); - // Ensure joint states are available if (joint_angle.empty()) { @@ -106,40 +87,19 @@ void MoveitServer::rotate_effector_callback( response->success = false; return; } - - int count = 5; - - // set_constraints(); + RCLCPP_INFO(this->get_logger(), "Received RotateEffector request to rotate end effector by angle: %f", request->rotation_angle); // Convert joint_angle (std::vector) to std::vector std::vector target_joint_values = {joint_angle[2], joint_angle[0], joint_angle[1], joint_angle[3], joint_angle[4], joint_angle[5] + (request->rotation_angle)}; - // Update the last joint (assumed to be joint6) by adding the rotation - - // target_joint_values[5] += request->rotation_angle; // Add rotation in radians to joint6 - RCLCPP_INFO(this->get_logger(), "Updated joint6 angle: %f", target_joint_values[5]); - - for (int i = 0; i < count; i++) - { - move_group_arm->setJointValueTarget(target_joint_values); - moveit::planning_interface::MoveGroupInterface::Plan my_plan3; - bool success = (move_group_arm->plan(my_plan3) == moveit::core::MoveItErrorCode::SUCCESS); - trajectory_msg = my_plan3.trajectory_.joint_trajectory; - - if (success) - { - move_group_arm->execute(my_plan3); - trajectory_pub->publish(trajectory_msg); - response->success = true; - RCLCPP_INFO(this->get_logger(), "End effector rotation executed successfully."); - break; - } - } + move_group_arm->setJointValueTarget(target_joint_values); + configure_move_group(planning_parameters_.trajectory); + move_group_arm->clearPathConstraints(); + response->success = plan_and_execute_with_retries(); } void MoveitServer::sync_callback(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(this->get_logger(), "MoveitServer::sync_callback called"); std::vector joint_values; for (const auto &x : request->joint_state.position) { @@ -147,40 +107,9 @@ void MoveitServer::sync_callback(const std::shared_ptrsetJointValueTarget(joint_values); - move_group_arm->setMaxVelocityScalingFactor(0.7); - move_group_arm->setMaxAccelerationScalingFactor(0.7); - // testing purpose - move_group_arm->setWorkspace(-1.0, -1.0, -1.0, 1.0, 1.0, 1.0); - - // move_group_arm->setPlannerId("RRTConnectkConfigDefault"); - - move_group_arm->setNumPlanningAttempts(5); - move_group_arm->setPlanningTime(2); - move_group_arm->setGoalTolerance(0.01); - move_group_arm->setGoalOrientationTolerance(0.035); + configure_move_group(planning_parameters_.sync); move_group_arm->clearPathConstraints(); - - moveit::planning_interface::MoveGroupInterface::Plan my_plan; - bool result = (move_group_arm->plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); - if (result) - { - result = (move_group_arm->execute(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - if (result) - { - RCLCPP_INFO(this->get_logger(), "Trajectory executed successfully"); - response->successq = true; - } - else - { - RCLCPP_ERROR(this->get_logger(), "Trajectory not executed"); - response->successq = false; - } - } - else - { - RCLCPP_WARN(this->get_logger(), "Planning for trajectory failed"); - response->successq = false; - } + response->successq = plan_and_execute_with_retries(false); } void MoveitServer::joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr msg) // <-- Updated this line From 20105fa7d55515fb6c072d6f82b373aa48d70a87 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Fri, 14 Feb 2025 19:07:15 +0100 Subject: [PATCH 3/3] parameter added for planner_id --- ras_moveit/config/tune_params.yaml | 26 +++++++++++++++++++ .../include/ras_moveit/moveit_server.hpp | 1 + ras_moveit/src/moveit_server.cpp | 4 ++- 3 files changed, 30 insertions(+), 1 deletion(-) diff --git a/ras_moveit/config/tune_params.yaml b/ras_moveit/config/tune_params.yaml index 9796a23..17bac1c 100644 --- a/ras_moveit/config/tune_params.yaml +++ b/ras_moveit/config/tune_params.yaml @@ -28,3 +28,29 @@ moveit_server: maxx: 1.0 maxy: 1.0 maxz: 1.0 + planner_id: RRTConnect + + # Available planner id + # - SBL + # - EST + # - LBKPIECE + # - BKPIECE + # - KPIECE + # - RRT + # - RRTConnect + # - RRTstar + # - TRRT + # - PRM + # - PRMstar + # - FMT + # - BFMT + # - PDST + # - STRIDE + # - BiTRRT + # - LBTRRT + # - BiEST + # - ProjEST + # - LazyPRM + # - LazyPRMstar + # - SPARS + # - SPARStwo diff --git a/ras_moveit/include/ras_moveit/moveit_server.hpp b/ras_moveit/include/ras_moveit/moveit_server.hpp index a755ede..c736409 100644 --- a/ras_moveit/include/ras_moveit/moveit_server.hpp +++ b/ras_moveit/include/ras_moveit/moveit_server.hpp @@ -85,6 +85,7 @@ struct PlanningParameters Workspace workspace; MotionFactor sync; MotionFactor trajectory; + std::string planner_id; }; class MoveitServer : public rclcpp::Node, public std::enable_shared_from_this diff --git a/ras_moveit/src/moveit_server.cpp b/ras_moveit/src/moveit_server.cpp index a19bdcc..59aee02 100644 --- a/ras_moveit/src/moveit_server.cpp +++ b/ras_moveit/src/moveit_server.cpp @@ -31,6 +31,7 @@ MoveitServer::MoveitServer(std::shared_ptr move_group_node) this->declare_parameter("collision_object_frame", "world"); this->declare_parameter("base_frame_id", "link_base"); this->declare_parameter("end_effector_frame_id", "link_eef"); + this->declare_parameter("planner_id", "RRTConnect"); this->declare_parameter("orientation_tolerance.x", 0.75); this->declare_parameter("orientation_tolerance.y", 0.75); this->declare_parameter("orientation_tolerance.z", 0.75); @@ -69,6 +70,7 @@ MoveitServer::MoveitServer(std::shared_ptr move_group_node) planning_parameters_.sync.goal_orientation_tolerance = this->get_parameter("sync.goal_orientation_tolerance").as_double(); planning_parameters_.sync.velocity_scaling_factor = this->get_parameter("sync.velocity_scaling_factor").as_double(); planning_parameters_.sync.acceleration_scaling_factor = this->get_parameter("sync.acceleration_scaling_factor").as_double(); + planning_parameters_.planner_id = this->get_parameter("planner_id").as_string(); move_group_name = this->get_parameter("move_group_name").as_string(); @@ -187,7 +189,7 @@ bool MoveitServer::Execute(const geometry_msgs::msg::Pose target_pose, const Mot void MoveitServer::configure_move_group(const MotionFactor motion_factor) { move_group_arm->setWorkspace(planning_parameters_.workspace.minx, planning_parameters_.workspace.miny, planning_parameters_.workspace.minz, planning_parameters_.workspace.maxx, planning_parameters_.workspace.maxy, planning_parameters_.workspace.maxz); - // move_group_arm->setPlannerId("RRTConnectkConfigDefault"); // TODO(Sachin): Check if this planner is defined already + move_group_arm->setPlannerId(planning_parameters_.planner_id); move_group_arm->setNumPlanningAttempts(motion_factor.planning_attempts); move_group_arm->setPlanningTime(motion_factor.planning_time); move_group_arm->setGoalTolerance(motion_factor.goal_tolerance);