Skip to content

Using Yaml file for param tuning #17

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
sachin-acceleraton-robotics wants to merge 3 commits into
base: dev
Choose a base branch
from
Open

Using Yaml file for param tuning #17

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
1a3bb70
yaml file for param tuning added
sachin-acceleraton-robotics Feb 13, 2025
ce74fab
use case params added for trajectory and sync
sachin-acceleraton-robotics Feb 14, 2025
20105fa
parameter added for planner_id
sachin-acceleraton-robotics Feb 14, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 6 additions & 6 deletions ras_moveit/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@ find_package(std_srvs REQUIRED)

install(
DIRECTORY
config
include
launch
DESTINATION
Expand All @@ -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
Expand All @@ -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()
Expand Down
56 changes: 56 additions & 0 deletions ras_moveit/config/tune_params.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,56 @@
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
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
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
94 changes: 0 additions & 94 deletions ras_moveit/include/moveit_server.hpp
View file
Open in desktop

This file was deleted.

137 changes: 137 additions & 0 deletions ras_moveit/include/ras_moveit/moveit_server.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,137 @@
/*
*
* 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 <https://www.gnu.org/licenses/>.
*
* For inquiries or further information, you may contact:
* Harsh Davda
* Email: info@opensciencestack.org
*/

#ifndef MOVEIT_SERVER_HPP
#define MOVEIT_SERVER_HPP

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/msg/display_robot_state.hpp>
#include <moveit_msgs/msg/display_trajectory.hpp>
#include <trajectory_msgs/msg/joint_trajectory.hpp>
#include <ras_interfaces/srv/pose_req.hpp>
#include <ras_interfaces/srv/joint_sat.hpp>
#include <ras_interfaces/srv/load_exp.hpp>
#include <std_srvs/srv/set_bool.hpp>
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_stamped.h>
#include <geometry_msgs/msg/point.h>
#include <moveit_msgs/msg/constraints.h>
#include <moveit_msgs/msg/orientation_constraint.h>
#include <moveit_msgs/msg/position_constraint.h>
#include <moveit_msgs/msg/robot_state.h>
#include <moveit_msgs/msg/workspace_parameters.h>
#include <string>
#include <vector>
#include <ras_interfaces/srv/rotate_effector.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <geometric_shapes/shapes.h>
#include <geometric_shapes/shape_operations.h>
#include <ras_interfaces/srv/joint_req.hpp>
#include <ras_interfaces/srv/action_traj.hpp>

using moveit::planning_interface::MoveGroupInterface;


struct Tolerance
{
double x;
double y;
double z;
};

struct Workspace
{
double minx;
double miny;
double minz;
double maxx;
double maxy;
double maxz;
};

struct MotionFactor
{
double velocity_scaling_factor;
double acceleration_scaling_factor;
int planning_attempts;
double planning_time;
double goal_tolerance;
double goal_orientation_tolerance;
};

struct PlanningParameters
{
Tolerance orientation_tolerance;
Workspace workspace;
MotionFactor sync;
MotionFactor trajectory;
std::string planner_id;
};

class MoveitServer : public rclcpp::Node, public std::enable_shared_from_this<MoveitServer>
{
public:
MoveitServer(std::shared_ptr<rclcpp::Node> move_group_node);
~MoveitServer();
void move_to_pose_callback(const std::shared_ptr<ras_interfaces::srv::PoseReq::Request> request, std::shared_ptr<ras_interfaces::srv::PoseReq::Response> response);
void move_to_joint_callback(const std::shared_ptr<ras_interfaces::srv::JointReq::Request> request, std::shared_ptr<ras_interfaces::srv::JointReq::Response> response);
void rotate_effector_callback(const std::shared_ptr<ras_interfaces::srv::RotateEffector::Request> request, std::shared_ptr<ras_interfaces::srv::RotateEffector::Response> response);
void joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr msg);
void sync_callback(const std::shared_ptr<ras_interfaces::srv::JointSat::Request> request, std::shared_ptr<ras_interfaces::srv::JointSat::Response> response);
void set_constraints(const geometry_msgs::msg::Pose::_orientation_type &quat);
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<ras_interfaces::srv::ActionTraj::Request> request,
std::shared_ptr<ras_interfaces::srv::ActionTraj::Response> response);
// void trajectory_callback(const std::shared_ptr<ras_interfaces::srv::ActionTraj::Request> request,
// std::shared_ptr<ras_interfaces::srv::ActionTraj::Response> response);
void AddScenePlane();

private:
void configure_move_group(const MotionFactor motion_factor);
bool plan_and_execute_with_retries(bool publish_trajectory = true);

std::shared_ptr<moveit::planning_interface::MoveGroupInterface> 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<ras_interfaces::srv::PoseReq>::SharedPtr move_to_pose_srv_;
rclcpp::Service<ras_interfaces::srv::JointReq>::SharedPtr move_to_joint_srv_;
rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr trajectory_pub;
rclcpp::Service<ras_interfaces::srv::RotateEffector>::SharedPtr rotate_effector_srv_;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
rclcpp::Service<ras_interfaces::srv::JointSat>::SharedPtr sync_srv;
rclcpp::Service<ras_interfaces::srv::ActionTraj>::SharedPtr execute_traj_srv;

std::vector<float> 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
22 changes: 13 additions & 9 deletions ras_moveit/launch/moveit_real_server.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,22 +3,26 @@
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',
name='moveit_server',
output='screen',
parameters=[
{'move_group_name': robot_component.movegroup_name},
]
),
params_file
]),

])
])
Loading