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

add cartesian path planner

Browse source
This commit is contained in:
Ilya Uraev 2022-03-16 00:50:12 +04:00
parent ddba530489
commit 387e4ca428
8 changed files with 244 additions and 31 deletions
Download patch file Download diff file Expand all files Collapse all files

35
rasmt_moveit_config/launch/rasmt_moveit.launch.py
View file

@ -174,28 +174,29 @@ def generate_launch_description():
]
)
# move_to_joint_state_action_server = Node(
# package="robossembler_servers",
# executable="move_to_joint_states_action_server",
# name="joint_states_moveit",
# parameters=[
# robot_description,
# robot_description_semantic,
# kinematics_yaml,
# robot_description_planning,
# ompl_yaml,
# planning,
# trajectory_execution,
# moveit_controllers,
# planning_scene_monitor_parameters,
# use_sim_time
# ]
# )
move_cartesian_path_action_server = Node(
package="robossembler_servers",
executable="move_cartesian_path_action_server",
name="move_cartesian_path_action_server",
parameters=[
robot_description,
robot_description_semantic,
kinematics_yaml,
robot_description_planning,
ompl_yaml,
planning,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
use_sim_time
]
)
launch_nodes = []
launch_nodes.append(rviz)
launch_nodes.append(move_group_node)
launch_nodes.append(move_topose_action_server)
launch_nodes.append(move_cartesian_path_action_server)
# launch_nodes.append(move_to_joint_state_action_server)

3
rasmt_support/launch/rasmt_gazebo.launch.py
View file

@ -19,6 +19,7 @@ def generate_launch_description():
))
world_file = os.path.join(get_package_share_directory("rasmt_support"), "world", "robossembler.world")
#test_world_file = "/home/bill-finger/gazebo_pkgs_ws/gazebo-pkgs/gazebo_grasp_plugin/test_world/test_world_full.world"
gzserver = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
@ -27,7 +28,7 @@ def generate_launch_description():
"launch",
"gzserver.launch.py"
])
), launch_arguments={'world':world_file}.items()
), launch_arguments={'world':world_file, 'extra_gazebo_args':'--verbose'}.items()
)
gzclient = IncludeLaunchDescription(
PythonLaunchDescriptionSource(

5
robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml
View file

@ -7,9 +7,10 @@
<Action ID="GetEntityState" part_name="${arg2}" server_name="/get_entity_state" server_timeout="10"/>
<Action ID="MoveToPose" command="go_to" arm_group="${arg0}" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperCmd" command="open" server_name="/gripper_cmd" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" command="pick" arm_group="${arg0}" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" command="down" arm_group="${arg0}" server_name="/move_cartesian" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperCmd" command="close" server_name="/gripper_cmd" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" command="go_to" arm_group="${arg0}" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" command="up" arm_group="${arg0}" server_name="/move_cartesian" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperCmd" command="open" server_name="/gripper_cmd" server_timeout="10" cancel_timeout="5" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->

2
robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp
View file

@ -15,7 +15,7 @@ class SendGripperCmd : public BtAction<GripperCmd>
: BtAction<GripperCmd>(name, config) {
//gripper_gap = 0.02;
gripper_point_a_ = 0.06;
gripper_point_b_ = 0.025;
gripper_point_b_ = 0.05;
gripper_cmd.insert(std::make_pair("open", gripper_point_a_));
gripper_cmd.insert(std::make_pair("close", gripper_point_b_));

23
robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp
View file

@ -8,7 +8,7 @@
using namespace BT;
using MoveToPoseAction = robossembler_interfaces::action::MoveitSendPose;
static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveToPointActionClient");
static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveToPoseActionClient");
class MoveToPose : public BtAction<MoveToPoseAction>
{
@ -23,16 +23,25 @@ class MoveToPose : public BtAction<MoveToPoseAction>
target_pose_a_.orientation.z = 0.0;
target_pose_a_.orientation.w = 0.009;
target_pose_b_.position.x = 0.40644;
target_pose_b_.position.x = 0.0;
target_pose_b_.position.y = 0.0;
target_pose_b_.position.z = 0.20;
target_pose_b_.position.z = -0.12;
target_pose_b_.orientation.x = 0;
target_pose_b_.orientation.y = 1.0;
target_pose_b_.orientation.y = 0.0;
target_pose_b_.orientation.z = 0.0;
target_pose_b_.orientation.w = 0.009;
target_pose_b_.orientation.w = 0.0;
target_pose_c_.position.x = 0.0;
target_pose_c_.position.y = 0.0;
target_pose_c_.position.z = 0.12;
target_pose_c_.orientation.x = 0;
target_pose_c_.orientation.y = 0.0;
target_pose_c_.orientation.z = 0.0;
target_pose_c_.orientation.w = 0.0;
pick_and_place_command.insert(std::make_pair("go_to", target_pose_a_));
pick_and_place_command.insert(std::make_pair("pick", target_pose_b_));
pick_and_place_command.insert(std::make_pair("down", target_pose_b_));
pick_and_place_command.insert(std::make_pair("up", target_pose_c_));
}
@ -62,7 +71,7 @@ class MoveToPose : public BtAction<MoveToPoseAction>
private:
geometry_msgs::msg::Pose target_pose_a_;
geometry_msgs::msg::Pose target_pose_b_;
geometry_msgs::msg::Pose target_pose_b_, target_pose_c_;
std::string robot_name_;
std::map<std::string, geometry_msgs::msg::Pose> pick_and_place_command;
std::string command;

181
robossembler_servers/src/move_cartesian_path_action_server.cpp Normal file
View file

@ -0,0 +1,181 @@
#include <functional>
#include <memory>
#include <thread>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/timer.hpp"
#include "rclcpp_components/register_node_macro.hpp"
// action libs
#include "rclcpp_action/rclcpp_action.hpp"
#include "robossembler_interfaces/msg/action_feedback_status_constants.hpp"
#include "robossembler_interfaces/action/moveit_send_pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "geometry_msgs/msg/transform.hpp"
// moveit libs
#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 <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
*/
// For Visualization
//#include <eigen_conversions/eigen_msg.h>
#include "moveit_msgs/msg/display_robot_state.hpp"
#include "moveit_msgs/msg/display_trajectory.hpp"
#include "moveit_msgs/msg/robot_trajectory.hpp"
#include "moveit_msgs/action/move_group.hpp"
namespace robossembler_actions
{
class MoveCartesianActionServer : public rclcpp::Node
{
public:
using MoveitSendPose = robossembler_interfaces::action::MoveitSendPose;
//explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr& node)
explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_cartesian_action_server"), node_(node)
{
// using namespace std::placeholders;
// this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
// this->get_node_base_interface(),
// this->get_node_clock_interface(),
// this->get_node_logging_interface(),
// this->get_node_waitables_interface(),
// "move_topose",
// std::bind(&MoveCartesianActionServer::goal_callback, this, _1, _2),
// std::bind(&MoveCartesianActionServer::cancel_callback, this, _1),
// std::bind(&MoveCartesianActionServer::accepted_callback, this, _1));
}
void init()
{
action_server_ = rclcpp_action::create_server<MoveitSendPose>(
node_->get_node_base_interface(),
node_->get_node_clock_interface(),
node_->get_node_logging_interface(),
node_->get_node_waitables_interface(),
"move_cartesian",
std::bind(&MoveCartesianActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2),
std::bind(&MoveCartesianActionServer::cancel_callback, this, std::placeholders::_1),
std::bind(&MoveCartesianActionServer::accepted_callback, this, std::placeholders::_1)
);
}
private:
rclcpp::Node::SharedPtr node_;
rclcpp_action::Server<MoveitSendPose>::SharedPtr action_server_;
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendPose>;
rclcpp_action::GoalResponse goal_callback(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const MoveitSendPose::Goal> goal)
{
RCLCPP_INFO(this->get_logger(), "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, %f, %f]",
goal->robot_name.c_str(), goal->target_pose.position.x, goal->target_pose.position.y, goal->target_pose.position.z,
goal->target_pose.orientation.x, goal->target_pose.orientation.y, goal->target_pose.orientation.z,
goal->target_pose.orientation.w);
(void)uuid;
if (false) {
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Received cancel request");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle)
{
using namespace std::placeholders;
std::thread(std::bind(&MoveCartesianActionServer::execute, this, _1), goal_handle).detach();
}
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing action goal");
const auto goal = goal_handle->get_goal();
auto result = std::make_shared<MoveitSendPose::Result>();
moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
moveit::core::RobotState start_state(*move_group_interface.getCurrentState());
auto start_pose = move_group_interface.getCurrentPose();
std::vector<geometry_msgs::msg::Pose> waypoints;
RCLCPP_INFO(this->get_logger(), "Current with Pose [%f, %f, %f, %f, %f, %f, %f]",
start_pose.pose.position.x, start_pose.pose.position.y, start_pose.pose.position.z,
start_pose.pose.orientation.x, start_pose.pose.orientation.y, start_pose.pose.orientation.z,
start_pose.pose.orientation.w);
//waypoints.push_back(start_pose.pose);
geometry_msgs::msg::Pose target_pose = start_pose.pose;
//goal->target_pose
target_pose.position.z = target_pose.position.z + goal->target_pose.position.z;
target_pose.position.y = target_pose.position.y + goal->target_pose.position.y;
target_pose.position.x = target_pose.position.x + goal->target_pose.position.x;
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.01;
double fraction = move_group_interface.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
waypoints.clear();
if(fraction>0)
{
RCLCPP_INFO(this->get_logger(), "Planning success");
move_group_interface.execute(trajectory);
//move_group_interface.move();
}else{
RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
}
if (goal_handle->is_canceling()) {
goal_handle->canceled(result);
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
return;
}
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
}
}; // class MoveCartesianActionServer
}// namespace robossembler_actions
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto node = rclcpp::Node::make_shared("move_cartesian", "", node_options);
robossembler_actions::MoveCartesianActionServer server(node);
std::thread run_server([&server]() {
rclcpp::sleep_for(std::chrono::seconds(3));
server.init();
});
rclcpp::spin(node);
run_server.join();
return 0;
}

4
robossembler_servers/src/move_topose_action_server.cpp
View file

@ -112,9 +112,7 @@ private:
auto result = std::make_shared<MoveitSendPose::Result>();
moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
std::copy(move_group_interface.getJointModelGroupNames().begin(), move_group_interface.getJointModelGroupNames().end(),
std::ostream_iterator<std::string>(std::cout, ", "));
move_group_interface.setStartState(*move_group_interface.getCurrentState());
move_group_interface.setPoseTarget(goal->target_pose);
move_group_interface.setMaxVelocityScalingFactor(goal->end_effector_velocity);

22
sdf_models/models/cube/model.sdf
View file

@ -1,14 +1,36 @@
<?xml version="1.0" ?>
<sdf version="1.5">
<model name="cube">
<static>0</static>
<!--gravity>0</gravity-->
<pose>0 0 0 0 0 0</pose>
<link name="link">
<inertial>
<pose>0.000000 0.000000 0.000000 0.000000 -0.000000 -0.000000</pose>
<mass>0.005709</mass>
<inertia>
<ixx>0.000001</ixx>
<ixy>-0.000000</ixy>
<ixz>-0.000000</ixz>
<iyy>0.000002</iyy>
<iyz>-0.000000</iyz>
<izz>0.000001</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
<surface>
<friction>
<ode>
<mu>10000</mu>
<mu2>10000</mu2>
</ode>
</friction>
</surface>
</collision>
<visual name="visual">
<geometry>