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

add skill launch, refactor pick-place pose loader

Browse source
This commit is contained in:
Ilya Uraev 2023-11-30 13:47:33 +03:00
parent 034e172f62
commit d72c06efea
14 changed files with 1032 additions and 191 deletions
Download patch file Download diff file Expand all files Collapse all files

9
rbs_skill_servers/CMakeLists.txt
View file

@ -32,6 +32,8 @@ find_package(tf2_eigen REQUIRED)
find_package(tf2_msgs REQUIRED)
find_package(tinyxml2_vendor REQUIRED)
find_package(TinyXML2 REQUIRED)
find_package (Eigen3 3.3 REQUIRED)
# Default to Fortress
set(SDF_VER 12)
@ -114,7 +116,7 @@ target_compile_definitions(pick_place_pose_loader
PRIVATE "PICK_PLACE_POSE_LOADER_CPP_BUILDING_DLL")
ament_target_dependencies(gripper_action_server ${deps})
ament_target_dependencies(pick_place_pose_loader ${deps})
ament_target_dependencies(pick_place_pose_loader ${deps} Eigen3)
rclcpp_components_register_node(gripper_action_server PLUGIN "rbs_skill_actions::GripperControlActionServer" EXECUTABLE gripper_control_action_server)
rclcpp_components_register_node(pick_place_pose_loader PLUGIN "rbs_skill_actions::GetGraspPickPoseServer" EXECUTABLE pick_place_pose_loader_service_server)
@ -155,6 +157,11 @@ install(
DESTINATION include
)
install(
DIRECTORY launch config
DESTINATION share/${PROJECT_NAME}
)
install(
TARGETS
move_topose_action_server

73
rbs_skill_servers/config/gripperPositions.yaml Normal file
View file

@ -0,0 +1,73 @@
box1:
PreGraspPose: [0.25, 0.55, 0.6515, -0.707, 0.0, 0.0, 0.707]
GraspPose: [0.25, 0.655, 0.6515, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [0.25, 0.655, 0.67, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [0.25, 0.5, 0.67, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, -0.055, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, -0.055, 0.46, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, -0.055, 0.6, 1.0, 0.0, 0.0, 0.0]
box2:
PreGraspPose: [0.0, 0.55, 0.6515, -0.707, 0.0, 0.0, 0.707]
GraspPose: [0.0, 0.655, 0.6515, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [0.0, 0.655, 0.67, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [0.0, 0.5, 0.67, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, 0.0, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, 0.0, 0.46, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, 0.0, 0.6, 1.0, 0.0, 0.0, 0.0]
box3:
PreGraspPose: [-0.25, 0.55, 0.6515, -0.707, 0.0, 0.0, 0.707]
GraspPose: [-0.25, 0.655, 0.6515, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [-0.25, 0.655, 0.67, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [-0.25, 0.5, 0.67, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, 0.055, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, 0.055, 0.46, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, 0.055, 0.6, 1.0, 0.0, 0.0, 0.0]
box4:
PreGraspPose: [0.25, 0.55, 0.3015, -0.707, 0.0, 0.0, 0.707]
GraspPose: [0.25, 0.655, 0.3015, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [0.25, 0.655, 0.32, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [0.25, 0.5, 0.32, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, 0.0275, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, 0.0275, 0.49, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, 0.0275, 0.6, 1.0, 0.0, 0.0, 0.0]
box5:
PreGraspPose: [0.0, 0.55, 0.3015, -0.707, 0.0, 0.0, 0.707]
GraspPose: [0.0, 0.655, 0.3015, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [0.0, 0.655, 0.32, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [0.0, 0.5, 0.32, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, -0.0275, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, -0.0275, 0.49, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, -0.0275, 0.6, 1.0, 0.0, 0.0, 0.0]
box6:
PreGraspPose: [-0.25, 0.55, 0.3015, -0.707, 0.0, 0.0, 0.707]
GraspPose: [-0.25, 0.655, 0.3015, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [-0.25, 0.655, 0.32, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [-0.25, 0.5, 0.32, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, 0.0, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, 0.0, 0.56, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, 0.0, 0.6, 1.0, 0.0, 0.0, 0.0]
pre_place_joint_states:
- 0.11030024152330242
- 0.8306162497371689
- -1.7680363334650195
- -0.6262361658856159
- 1.4713289344704141
- -0.11066274809566562
scene_objects:
- box1
- box2
- box3
- box4
- box5
- box6
- last

56
rbs_skill_servers/include/rbs_skill_servers/pick_place_pose_loader.hpp
View file

@ -1,29 +1,39 @@
#include <rclcpp/rclcpp.hpp>
#include "rclcpp_components/register_node_macro.hpp"
#include <memory>
#include <algorithm>
#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
#include "rclcpp/rclcpp.hpp"
#include "moveit_msgs/msg/grasp.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <algorithm>
#include <geometry_msgs/msg/detail/transform_stamped__struct.hpp>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_ros/transform_listener.h>
namespace rbs_skill_actions
{
class GetGraspPickPoseServer : public rclcpp::Node
{
public:
explicit GetGraspPickPoseServer(rclcpp::NodeOptions options);
private:
static std::vector<geometry_msgs::msg::Pose> collect_pose(const Eigen::Affine3d pose, const geometry_msgs::msg::Vector3 direction, const Eigen::Vector3d scale_vec);
rclcpp::Service<rbs_skill_interfaces::srv::GetPickPlacePoses>::SharedPtr srv_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
geometry_msgs::msg::TransformStamped tf_data;
void handle_server(const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr request,
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr response);
std::vector<double> grasp_param_pose;
Eigen::Affine3d get_Affine_from_arr(const std::vector<double> pose);
};
}
namespace rbs_skill_actions {
class GetGraspPickPoseServer : public rclcpp::Node {
public:
explicit GetGraspPickPoseServer(rclcpp::NodeOptions options);
private:
static std::vector<geometry_msgs::msg::Pose>
collect_pose(const Eigen::Isometry3d &graspPose,
const geometry_msgs::msg::Vector3 &move_direction,
const Eigen::Vector3d &scale_vec);
rclcpp::Service<rbs_skill_interfaces::srv::GetPickPlacePoses>::SharedPtr srv_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
geometry_msgs::msg::TransformStamped place_pose_tf;
geometry_msgs::msg::TransformStamped grasp_pose_tf;
void handle_server(
const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr
request,
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr
response);
std::vector<double> grasp_param_pose;
Eigen::Affine3d get_Affine_from_arr(const std::vector<double> pose);
};
} // namespace rbs_skill_actions
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GetGraspPickPoseServer);

117
rbs_skill_servers/launch/skills.launch.py Normal file
View file

@ -0,0 +1,117 @@
import os
from launch import LaunchDescription, LaunchContext
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
ExecuteProcess,
OpaqueFunction
)
from ament_index_python.packages import get_package_share_directory
from launch.conditions import IfCondition, 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
from rbs_launch_utils.launch_common import load_yaml
def generate_launch_description():
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"robot_description",
default_value="",
description="robot description string",
)
)
declared_arguments.append(
DeclareLaunchArgument("robot_description_semantic", default_value="")
)
declared_arguments.append(
DeclareLaunchArgument("robot_description_kinematics", default_value="")
)
declared_arguments.append(
DeclareLaunchArgument("use_sim_time", default_value="")
)
declared_arguments.append(
DeclareLaunchArgument("with_gripper_condition", default_value="")
)
declared_arguments.append(
DeclareLaunchArgument("points_params_filepath", default_value="")
)
robot_description_decl = LaunchConfiguration("robot_description")
robot_description_semantic_decl = LaunchConfiguration("robot_description_semantic")
robot_description_kinematics = LaunchConfiguration("robot_description_kinematics")
use_sim_time = LaunchConfiguration("use_sim_time")
with_gripper_condition = LaunchConfiguration("with_gripper_condition")
points_params_filepath_decl = LaunchConfiguration("points_params_filepath")
robot_description = {"robot_description": robot_description_decl}
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_decl}
points_params = load_yaml(
"rbs_skill_servers", "config/gripperPositions.yaml"
)
move_topose_action_server = Node(
package="rbs_skill_servers",
executable="move_topose_action_server",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
gripper_control_node = Node(
package="rbs_skill_servers",
executable="gripper_control_action_server",
parameters= [
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
],
condition=IfCondition(with_gripper_condition)
)
move_cartesian_path_action_server = Node(
package="rbs_skill_servers",
executable="move_cartesian_path_action_server",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
move_joint_state_action_server = Node(
package="rbs_skill_servers",
executable="move_to_joint_states_action_server",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
grasp_pose_loader = Node(
package="rbs_skill_servers",
executable="pick_place_pose_loader_service_server",
output="screen"
)
nodes_to_start =[
move_topose_action_server,
gripper_control_node,
move_cartesian_path_action_server,
move_joint_state_action_server,
grasp_pose_loader
]
return LaunchDescription(declared_arguments + nodes_to_start)

203
rbs_skill_servers/src/pick_place_pose_loader.cpp
View file

@ -1,110 +1,125 @@
//#include "rbs_skill_servers"
#include "rbs_skill_servers/pick_place_pose_loader.hpp"
#include <Eigen/src/Core/Matrix.h>
#include <Eigen/src/Geometry/AngleAxis.h>
#include <Eigen/src/Geometry/Quaternion.h>
#include <Eigen/src/Geometry/Transform.h>
#include <cmath>
#include <rclcpp/logging.hpp>
#include <tf2_eigen/tf2_eigen.hpp>
// #include <Eigen/src/Core/Matrix.h>
// #include <Eigen/src/Geometry/Transform.h>
// #include <rclcpp/logging.hpp>
// #include <tf2/LinearMath/Quaternion.h>
// #include <tf2/exceptions.h>
// #include <tf2/time.h>
// #include <tf2_eigen/tf2_eigen.hpp>
using GetGraspPlacePoseServer = rbs_skill_actions::GetGraspPickPoseServer;
using GetGraspPlacePoseService = rbs_skill_interfaces::srv::GetPickPlacePoses;
using namespace std::chrono_literals;
// rclcpp::Node::SharedPtr g_node = nullptr;
GetGraspPlacePoseServer::GetGraspPickPoseServer(rclcpp::NodeOptions options)
: Node("grasp_place_pose_loader", options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true))
{
tf_buffer_ =
std::make_unique<tf2_ros::Buffer>(this->get_clock());
tf_listener_ =
std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
: Node("grasp_place_pose_loader",
options.allow_undeclared_parameters(true)
.automatically_declare_parameters_from_overrides(true)) {
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
srv_ = create_service<GetGraspPlacePoseService>("/get_pick_place_pose_service",
std::bind(&GetGraspPickPoseServer::handle_server, this, std::placeholders::_1, std::placeholders::_2));
srv_ = create_service<GetGraspPlacePoseService>(
"/get_pick_place_pose_service",
std::bind(&GetGraspPickPoseServer::handle_server, this,
std::placeholders::_1, std::placeholders::_2));
}
void
GetGraspPlacePoseServer::handle_server(const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr request,
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr response)
{
std::string rrr = "ASSEMBLE_" + request->object_name; // TODO: replace with better name
try {
tf_data = tf_buffer_->lookupTransform(
"world", rrr.c_str(),
tf2::TimePointZero);
} catch (const tf2::TransformException & ex) {
RCLCPP_ERROR(
this->get_logger(), "Could not transform %s to %s: %s",
rrr.c_str(), "world", ex.what());
return;
}
// TODO: Here need to check the parameter
// We can use another parameter library from PickNik
grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose").as_double_array();
RCLCPP_INFO(this->get_logger(), "\nGrasp direction \n x: %.2f \n y: %.2f \n z: %.2f",
request->grasp_direction.x,
request->grasp_direction.y,
request->grasp_direction.z);
RCLCPP_INFO(this->get_logger(), "\nPlace direction \n x: %.2f \n y: %.2f \n z: %.2f",
request->place_direction.x,
request->place_direction.y,
request->place_direction.z);
Eigen::Affine3d grasp_pose = get_Affine_from_arr(grasp_param_pose);
Eigen::Affine3d place_pose = tf2::transformToEigen(tf_data);
//std::cout << "grasp_point = " << std::endl << grasp_pose.translation() << std::endl << grasp_pose.linear() << std::endl;
//std::cout << "place_pose = " << std::endl << place_pose.translation() << std::endl << place_pose.linear() << std::endl;
Eigen::Vector3d vec_grasp(0.15,0.15,0.02);
Eigen::Vector3d vec_place(0,0,0.15);
response->grasp = collect_pose(grasp_pose, request->grasp_direction, vec_grasp);
response->place = collect_pose(place_pose, request->place_direction, vec_place);
void GetGraspPlacePoseServer::handle_server(
const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr
request,
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr
response) {
std::string object_name =
request->object_name + "_place"; // TODO: replace with better name
// Load place pose from TF2
try {
place_pose_tf = tf_buffer_->lookupTransform("world", object_name.c_str(),
tf2::TimePointZero);
} catch (const tf2::TransformException &ex) {
RCLCPP_ERROR(this->get_logger(), "Could not transform %s to %s: %s",
object_name.c_str(), "world", ex.what());
return;
}
// Load grasp pose from TF2
try {
grasp_pose_tf = tf_buffer_->lookupTransform(
"world", request->object_name.c_str(), tf2::TimePointZero);
} catch (const tf2::TransformException &ex) {
RCLCPP_ERROR(this->get_logger(), "Could not transforms %s to %s: %s",
request->object_name.c_str(), "world", ex.what());
}
RCLCPP_DEBUG(this->get_logger(),
"Grasp pose from tf \n\tx=%f\n\ty=%f\n\tz=%f",
grasp_pose_tf.transform.translation.x,
grasp_pose_tf.transform.translation.y,
grasp_pose_tf.transform.translation.z);
// TODO: Here need to check the parameter
// We can use another parameter library from PickNik
// grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose")
// .as_double_array();
RCLCPP_DEBUG(this->get_logger(),
"\nGrasp direction \n x: %.2f \n y: %.2f \n z: %.2f",
request->grasp_direction.x, request->grasp_direction.y,
request->grasp_direction.z);
RCLCPP_DEBUG(this->get_logger(),
"\nPlace direction \n x: %.2f \n y: %.2f \n z: %.2f",
request->place_direction.x, request->place_direction.y,
request->place_direction.z);
auto grasp_pose = tf2::transformToEigen(grasp_pose_tf);
auto place_pose = tf2::transformToEigen(place_pose_tf);
Eigen::Vector3d scale_grasp(0, 0, 0.10);
Eigen::Vector3d scale_place(0, 0, 0.15);
response->grasp =
collect_pose(grasp_pose, request->grasp_direction, scale_grasp);
response->place =
collect_pose(place_pose, request->place_direction, scale_place);
}
std::vector<geometry_msgs::msg::Pose>
GetGraspPlacePoseServer::collect_pose(
const Eigen::Affine3d pose,
const geometry_msgs::msg::Vector3 direction,
const Eigen::Vector3d scale_vec)
{
std::vector<geometry_msgs::msg::Pose> pose_v_;
pose_v_.push_back(tf2::toMsg(pose));
Eigen::Vector3d posedir;
Eigen::Affine3d pose_ = pose;
tf2::fromMsg(direction, posedir);
Eigen::Matrix3d Ixy = Eigen::Matrix3d::Zero();//posedir * posedir.transpose();
Ixy.diagonal() = posedir;
Eigen::Matrix3d Iz = Eigen::Matrix3d::Zero();
Iz.diagonal() = Eigen::Vector3d::UnitZ();
std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::collect_pose(
const Eigen::Isometry3d &graspPose,
const geometry_msgs::msg::Vector3 &move_direction,
const Eigen::Vector3d &scale_vec) {
std::vector<geometry_msgs::msg::Pose> poses{};
// Add GraspPose as base point
Eigen::Isometry3d grasp_pose{graspPose};
Eigen::Quaterniond theGraspOrientation(grasp_pose.linear());
theGraspOrientation =
theGraspOrientation * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());
grasp_pose.rotate(theGraspOrientation);
poses.push_back(tf2::toMsg(grasp_pose));
// PreGrasp calculation
Eigen::Vector3d preGraspTranslation =
static_cast<Eigen::Vector3d>(graspPose.translation()) + scale_vec;
Eigen::Quaterniond preGraspOrientation(graspPose.linear());
preGraspOrientation =
preGraspOrientation * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());
Eigen::Isometry3d preGraspPose = Eigen::Isometry3d::Identity();
preGraspPose.translate(preGraspTranslation);
preGraspPose.rotate(preGraspOrientation);
poses.push_back(tf2::toMsg(preGraspPose));
if (posedir.cwiseEqual(Eigen::Vector3d::UnitX()).all()) // IF Direction == [1,0,0]
{
std::cout << "\n TBD" << std::endl;
}
else if (posedir.cwiseEqual(Eigen::Vector3d::UnitY()).all()) // IF Direction == [0,1,0]
{
// Gp -- grasp point frame
pose_.pretranslate(-(Ixy * scale_vec)); // Gp-y
pose_v_.push_back(tf2::toMsg(pose_));
pose_.pretranslate(Iz * scale_vec); // Gp-y + z
pose_v_.push_back(tf2::toMsg(pose_));
pose_.pretranslate(Ixy * scale_vec); // Gp+z
pose_v_.push_back(tf2::toMsg(pose_));
}
else if (posedir.cwiseEqual(Eigen::Vector3d::UnitZ()).all()) // IF Direction == [0,0,1]
{
pose_.pretranslate(Ixy * scale_vec); // Choose Pre Grasp == Post Grasp
pose_v_.push_back(tf2::toMsg(pose_)); // So calculate only Pre Grasp
}
else
{
std::cout << "\n TBD" << std::endl;
}
return pose_v_;
}
// PostGrasp calculation
Eigen::Vector3d postGraspTranslation =
static_cast<Eigen::Vector3d>(graspPose.translation()) + scale_vec;
Eigen::Quaterniond postGraspOrientation(graspPose.linear());
postGraspOrientation =
postGraspOrientation * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());
Eigen::Affine3d
GetGraspPlacePoseServer::get_Affine_from_arr(const std::vector<double> pose)
{
Eigen::Vector3d point(std::vector<double>(pose.begin(), pose.begin()+3).data());
Eigen::Quaterniond quat(std::vector<double>(pose.begin()+3, pose.end()).data());
Eigen::Affine3d aff;
aff.translation() = point;
aff.linear() = quat.toRotationMatrix();
return aff;
Eigen::Isometry3d postGraspPose = Eigen::Isometry3d::Identity();
postGraspPose.translate(postGraspTranslation);
postGraspPose.rotate(postGraspOrientation);
poses.push_back(tf2::toMsg(postGraspPose));
return poses;
}