robossembler/runtime
7
0
Fork 0
Code Issues 3 Pull requests Projects Releases Packages Wiki Activity Actions

Added rbs_gym package for RL & multi-robot launch setup

Browse source
This commit is contained in:
Ilya Uraev 2024-07-04 11:38:08 +00:00 • committed by Igor Brylyov
parent f92670cd0d
commit b58307dea1
103 changed files with 15170 additions and 653 deletions
Download patch file Download diff file Expand all files Collapse all files

43
rbs_skill_servers/src/move_cartesian_path_action_server.cpp
View file

@ -1,5 +1,7 @@
#include <functional>
#include <memory>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/time_parameterization.h>
#include <thread>
#include "rclcpp/rclcpp.hpp"
@ -19,6 +21,7 @@
#include "moveit/move_group_interface/move_group_interface.h"
#include "moveit/planning_interface/planning_interface.h"
#include "moveit/robot_model_loader/robot_model_loader.h"
#include "moveit/trajectory_processing/time_optimal_trajectory_generation.h"
/*
#include <tf2/LinearMath/Quaternion.h>
@ -122,23 +125,59 @@ private:
std::vector<geometry_msgs::msg::Pose> waypoints;
auto current_pose = move_group_interface.getCurrentPose();
// waypoints.push_back(current_pose.pose);
// geometry_msgs::msg::Pose start_pose = current_pose.pose;
geometry_msgs::msg::Pose target_pose = goal->target_pose;
// target_pose.position = goal->target_pose.position;
// int num_waypoints = 100;
// for (int i = 1; i <= num_waypoints; ++i) {
// geometry_msgs::msg::Pose intermediate_pose;
// double fraction = static_cast<double>(i) / (num_waypoints + 1);
//
// intermediate_pose.position.x =
// start_pose.position.x +
// fraction * (target_pose.position.x - start_pose.position.x);
// intermediate_pose.position.y =
// start_pose.position.y +
// fraction * (target_pose.position.y - start_pose.position.y);
// intermediate_pose.position.z =
// start_pose.position.z +
// fraction * (target_pose.position.z - start_pose.position.z);
//
// intermediate_pose.orientation = start_pose.orientation;
//
// waypoints.push_back(intermediate_pose);
// }
waypoints.push_back(target_pose);
RCLCPP_INFO(this->get_logger(), "New cartesian target pose [%f, %f, %f]",
target_pose.position.x, target_pose.position.y,
target_pose.position.z);
// waypoints.push_back(start_pose.pose);
moveit_msgs::msg::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
const double eef_step = 0.001;
double fraction = move_group_interface.computeCartesianPath(
waypoints, eef_step, jump_threshold, trajectory);
robot_trajectory::RobotTrajectory rt(
move_group_interface.getCurrentState()->getRobotModel(),
goal->robot_name);
rt.setRobotTrajectoryMsg(*move_group_interface.getCurrentState(), trajectory);
trajectory_processing::TimeOptimalTrajectoryGeneration tp;
bool su = tp.computeTimeStamps(rt);
rt.getRobotTrajectoryMsg(trajectory);
moveit::planning_interface::MoveGroupInterface::Plan plan;
plan.trajectory_ = trajectory;
if (fraction > 0) {
RCLCPP_INFO(this->get_logger(), "Planning success");
moveit::core::MoveItErrorCode execute_err_code =
move_group_interface.execute(trajectory);
move_group_interface.execute(plan);
if (execute_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");