mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Replace rosparam_shortcuts with generate_parameter_library (#403)
This commit is contained in:
parent
745d220efd
commit
615e8ef248
@ -7,13 +7,13 @@ set(CMAKE_CXX_EXTENSIONS OFF)
|
|||||||
|
|
||||||
find_package(ament_cmake REQUIRED)
|
find_package(ament_cmake REQUIRED)
|
||||||
|
|
||||||
|
find_package(generate_parameter_library REQUIRED)
|
||||||
find_package(moveit_core REQUIRED)
|
find_package(moveit_core REQUIRED)
|
||||||
find_package(moveit_ros_planning REQUIRED)
|
find_package(moveit_ros_planning REQUIRED)
|
||||||
find_package(moveit_ros_planning_interface REQUIRED)
|
find_package(moveit_ros_planning_interface REQUIRED)
|
||||||
find_package(moveit_task_constructor_core REQUIRED)
|
find_package(moveit_task_constructor_core REQUIRED)
|
||||||
find_package(moveit_task_constructor_msgs REQUIRED)
|
find_package(moveit_task_constructor_msgs REQUIRED)
|
||||||
find_package(rclcpp REQUIRED)
|
find_package(rclcpp REQUIRED)
|
||||||
find_package(rosparam_shortcuts REQUIRED)
|
|
||||||
find_package(tf2_eigen REQUIRED)
|
find_package(tf2_eigen REQUIRED)
|
||||||
|
|
||||||
set(THIS_PACKAGE_INCLUDE_DEPENDS
|
set(THIS_PACKAGE_INCLUDE_DEPENDS
|
||||||
@ -23,7 +23,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
|
|||||||
moveit_task_constructor_core
|
moveit_task_constructor_core
|
||||||
moveit_task_constructor_msgs
|
moveit_task_constructor_msgs
|
||||||
rclcpp
|
rclcpp
|
||||||
rosparam_shortcuts
|
|
||||||
tf2_eigen
|
tf2_eigen
|
||||||
)
|
)
|
||||||
|
|
||||||
@ -39,6 +38,11 @@ include_directories(include)
|
|||||||
# declare a demo consisting of a single cpp file
|
# declare a demo consisting of a single cpp file
|
||||||
function(demo name)
|
function(demo name)
|
||||||
add_executable(${PROJECT_NAME}_${name} src/${name}.cpp)
|
add_executable(${PROJECT_NAME}_${name} src/${name}.cpp)
|
||||||
|
set(parameter_filename ${name}_parameters)
|
||||||
|
if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/src/${parameter_filename}.yaml)
|
||||||
|
generate_parameter_library(${parameter_filename} src/${parameter_filename}.yaml)
|
||||||
|
target_link_libraries(${PROJECT_NAME}_${name} ${parameter_filename})
|
||||||
|
endif()
|
||||||
ament_target_dependencies(${PROJECT_NAME}_${name} ${THIS_PACKAGE_INCLUDE_DEPENDS})
|
ament_target_dependencies(${PROJECT_NAME}_${name} ${THIS_PACKAGE_INCLUDE_DEPENDS})
|
||||||
set_target_properties(${PROJECT_NAME}_${name} PROPERTIES OUTPUT_NAME ${name} PREFIX "")
|
set_target_properties(${PROJECT_NAME}_${name} PROPERTIES OUTPUT_NAME ${name} PREFIX "")
|
||||||
install(TARGETS ${PROJECT_NAME}_${name}
|
install(TARGETS ${PROJECT_NAME}_${name}
|
||||||
@ -55,6 +59,7 @@ demo(ik_clearance_cost)
|
|||||||
demo(fallbacks_move_to)
|
demo(fallbacks_move_to)
|
||||||
|
|
||||||
demo(pick_place_demo)
|
demo(pick_place_demo)
|
||||||
|
target_link_libraries(${PROJECT_NAME}_pick_place_task pick_place_demo_parameters)
|
||||||
target_link_libraries(${PROJECT_NAME}_pick_place_demo ${PROJECT_NAME}_pick_place_task)
|
target_link_libraries(${PROJECT_NAME}_pick_place_demo ${PROJECT_NAME}_pick_place_task)
|
||||||
|
|
||||||
install(DIRECTORY launch config
|
install(DIRECTORY launch config
|
||||||
|
|||||||
@ -35,7 +35,7 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
// ROS
|
// ROS
|
||||||
#include <rclcpp/rclcpp.hpp>
|
#include <rclcpp/node.hpp>
|
||||||
|
|
||||||
// MoveIt
|
// MoveIt
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
@ -57,6 +57,7 @@
|
|||||||
#include <moveit/task_constructor/solvers/cartesian_path.h>
|
#include <moveit/task_constructor/solvers/cartesian_path.h>
|
||||||
#include <moveit/task_constructor/solvers/pipeline_planner.h>
|
#include <moveit/task_constructor/solvers/pipeline_planner.h>
|
||||||
#include <moveit_task_constructor_msgs/action/execute_task_solution.hpp>
|
#include <moveit_task_constructor_msgs/action/execute_task_solution.hpp>
|
||||||
|
#include "pick_place_demo_parameters.hpp"
|
||||||
|
|
||||||
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
|
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
|
||||||
#include <tf2_eigen/tf2_eigen.hpp>
|
#include <tf2_eigen/tf2_eigen.hpp>
|
||||||
@ -70,56 +71,22 @@ namespace moveit_task_constructor_demo {
|
|||||||
using namespace moveit::task_constructor;
|
using namespace moveit::task_constructor;
|
||||||
|
|
||||||
// prepare a demo environment from ROS parameters under node
|
// prepare a demo environment from ROS parameters under node
|
||||||
void setupDemoScene(const rclcpp::Node::SharedPtr& node);
|
void setupDemoScene(const pick_place_task_demo::Params& params);
|
||||||
|
|
||||||
class PickPlaceTask
|
class PickPlaceTask
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
PickPlaceTask(const std::string& task_name, const rclcpp::Node::SharedPtr& pnh);
|
PickPlaceTask(const std::string& task_name);
|
||||||
~PickPlaceTask() = default;
|
~PickPlaceTask() = default;
|
||||||
|
|
||||||
bool init();
|
bool init(const rclcpp::Node::SharedPtr& node, const pick_place_task_demo::Params& params);
|
||||||
|
|
||||||
bool plan();
|
bool plan(const std::size_t max_solutions);
|
||||||
|
|
||||||
bool execute();
|
bool execute();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void loadParameters();
|
|
||||||
|
|
||||||
rclcpp::Node::SharedPtr node_;
|
|
||||||
|
|
||||||
std::string task_name_;
|
std::string task_name_;
|
||||||
moveit::task_constructor::TaskPtr task_;
|
moveit::task_constructor::TaskPtr task_;
|
||||||
|
|
||||||
// planning group properties
|
|
||||||
std::string arm_group_name_;
|
|
||||||
std::string eef_name_;
|
|
||||||
std::string hand_group_name_;
|
|
||||||
std::string hand_frame_;
|
|
||||||
|
|
||||||
// object + surface
|
|
||||||
std::vector<std::string> support_surfaces_;
|
|
||||||
std::string object_reference_frame_;
|
|
||||||
std::string surface_link_;
|
|
||||||
std::string object_name_;
|
|
||||||
std::string world_frame_;
|
|
||||||
std::vector<double> object_dimensions_;
|
|
||||||
|
|
||||||
// Predefined pose targets
|
|
||||||
std::string hand_open_pose_;
|
|
||||||
std::string hand_close_pose_;
|
|
||||||
std::string arm_home_pose_;
|
|
||||||
|
|
||||||
// Pick metrics
|
|
||||||
Eigen::Isometry3d grasp_frame_transform_;
|
|
||||||
double approach_object_min_dist_;
|
|
||||||
double approach_object_max_dist_;
|
|
||||||
double lift_object_min_dist_;
|
|
||||||
double lift_object_max_dist_;
|
|
||||||
|
|
||||||
// Place metrics
|
|
||||||
geometry_msgs::msg::Pose place_pose_;
|
|
||||||
double place_surface_offset_;
|
|
||||||
};
|
};
|
||||||
} // namespace moveit_task_constructor_demo
|
} // namespace moveit_task_constructor_demo
|
||||||
|
|||||||
@ -11,10 +11,10 @@
|
|||||||
<license>BSD</license>
|
<license>BSD</license>
|
||||||
|
|
||||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
<depend>generate_parameter_library</depend>
|
||||||
<depend>moveit_core</depend>
|
<depend>moveit_core</depend>
|
||||||
<depend>moveit_ros_planning_interface</depend>
|
<depend>moveit_ros_planning_interface</depend>
|
||||||
<depend>moveit_task_constructor_core</depend>
|
<depend>moveit_task_constructor_core</depend>
|
||||||
<depend>rosparam_shortcuts</depend>
|
|
||||||
<exec_depend>moveit_configs_utils</exec_depend>
|
<exec_depend>moveit_configs_utils</exec_depend>
|
||||||
<exec_depend>moveit_resources_panda_moveit_config</exec_depend>
|
<exec_depend>moveit_resources_panda_moveit_config</exec_depend>
|
||||||
<exec_depend>moveit_task_constructor_capabilities</exec_depend>
|
<exec_depend>moveit_task_constructor_capabilities</exec_depend>
|
||||||
|
|||||||
@ -8,7 +8,7 @@
|
|||||||
#include <moveit/task_constructor/stages/compute_ik.h>
|
#include <moveit/task_constructor/stages/compute_ik.h>
|
||||||
|
|
||||||
#include <moveit/task_constructor/cost_terms.h>
|
#include <moveit/task_constructor/cost_terms.h>
|
||||||
#include <rosparam_shortcuts/rosparam_shortcuts.h>
|
#include "ik_clearance_cost_parameters.hpp"
|
||||||
|
|
||||||
using namespace moveit::task_constructor;
|
using namespace moveit::task_constructor;
|
||||||
|
|
||||||
@ -18,6 +18,9 @@ int main(int argc, char** argv) {
|
|||||||
auto node = rclcpp::Node::make_shared("moveit_task_constructor_demo");
|
auto node = rclcpp::Node::make_shared("moveit_task_constructor_demo");
|
||||||
std::thread spinning_thread([node] { rclcpp::spin(node); });
|
std::thread spinning_thread([node] { rclcpp::spin(node); });
|
||||||
|
|
||||||
|
const auto param_listener = std::make_shared<ik_clearance_cost_demo::ParamListener>(node);
|
||||||
|
const auto params = param_listener->get_params();
|
||||||
|
|
||||||
Task t;
|
Task t;
|
||||||
t.stages()->setName("clearance IK");
|
t.stages()->setName("clearance IK");
|
||||||
|
|
||||||
@ -56,10 +59,8 @@ int main(int argc, char** argv) {
|
|||||||
ik->setMaxIKSolutions(100);
|
ik->setMaxIKSolutions(100);
|
||||||
|
|
||||||
auto cl_cost{ std::make_unique<cost::Clearance>() };
|
auto cl_cost{ std::make_unique<cost::Clearance>() };
|
||||||
cl_cost->cumulative = false;
|
cl_cost->cumulative = params.cumulative;
|
||||||
rosparam_shortcuts::get(node, "cumulative", cl_cost->cumulative); // sum up pairwise distances?
|
cl_cost->with_world = params.with_world;
|
||||||
cl_cost->with_world = true;
|
|
||||||
rosparam_shortcuts::get(node, "with_world", cl_cost->with_world); // consider distance to world objects?
|
|
||||||
ik->setCostTerm(std::move(cl_cost));
|
ik->setCostTerm(std::move(cl_cost));
|
||||||
|
|
||||||
t.add(std::move(ik));
|
t.add(std::move(ik));
|
||||||
|
|||||||
9
demo/src/ik_clearance_cost_parameters.yaml
Normal file
9
demo/src/ik_clearance_cost_parameters.yaml
Normal file
@ -0,0 +1,9 @@
|
|||||||
|
ik_clearance_cost_demo:
|
||||||
|
cumulative:
|
||||||
|
type: bool
|
||||||
|
default_value: false
|
||||||
|
read_only: true
|
||||||
|
with_world:
|
||||||
|
type: bool
|
||||||
|
default_value: true
|
||||||
|
read_only: true
|
||||||
@ -40,7 +40,7 @@
|
|||||||
// MTC pick/place demo implementation
|
// MTC pick/place demo implementation
|
||||||
#include <moveit_task_constructor_demo/pick_place_task.h>
|
#include <moveit_task_constructor_demo/pick_place_task.h>
|
||||||
|
|
||||||
#include <rosparam_shortcuts/rosparam_shortcuts.h>
|
#include "pick_place_demo_parameters.hpp"
|
||||||
|
|
||||||
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_demo");
|
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_demo");
|
||||||
|
|
||||||
@ -51,18 +51,20 @@ int main(int argc, char** argv) {
|
|||||||
auto node = rclcpp::Node::make_shared("moveit_task_constructor_demo", node_options);
|
auto node = rclcpp::Node::make_shared("moveit_task_constructor_demo", node_options);
|
||||||
std::thread spinning_thread([node] { rclcpp::spin(node); });
|
std::thread spinning_thread([node] { rclcpp::spin(node); });
|
||||||
|
|
||||||
moveit_task_constructor_demo::setupDemoScene(node);
|
const auto param_listener = std::make_shared<pick_place_task_demo::ParamListener>(node);
|
||||||
|
const auto params = param_listener->get_params();
|
||||||
|
moveit_task_constructor_demo::setupDemoScene(params);
|
||||||
|
|
||||||
// Construct and run pick/place task
|
// Construct and run pick/place task
|
||||||
moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", node);
|
moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task");
|
||||||
if (!pick_place_task.init()) {
|
if (!pick_place_task.init(node, params)) {
|
||||||
RCLCPP_INFO(LOGGER, "Initialization failed");
|
RCLCPP_INFO(LOGGER, "Initialization failed");
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (pick_place_task.plan()) {
|
if (pick_place_task.plan(params.max_solutions)) {
|
||||||
RCLCPP_INFO(LOGGER, "Planning succeded");
|
RCLCPP_INFO(LOGGER, "Planning succeded");
|
||||||
if (bool execute = false; rosparam_shortcuts::get(node, "execute", execute) && execute) {
|
if (params.execute) {
|
||||||
pick_place_task.execute();
|
pick_place_task.execute();
|
||||||
RCLCPP_INFO(LOGGER, "Execution complete");
|
RCLCPP_INFO(LOGGER, "Execution complete");
|
||||||
} else {
|
} else {
|
||||||
|
|||||||
97
demo/src/pick_place_demo_parameters.yaml
Normal file
97
demo/src/pick_place_demo_parameters.yaml
Normal file
@ -0,0 +1,97 @@
|
|||||||
|
pick_place_task_demo:
|
||||||
|
execute:
|
||||||
|
type: bool
|
||||||
|
default_value: false
|
||||||
|
table_name:
|
||||||
|
type: string
|
||||||
|
validation:
|
||||||
|
not_empty<>: []
|
||||||
|
table_reference_frame:
|
||||||
|
type: string
|
||||||
|
validation:
|
||||||
|
not_empty<>: []
|
||||||
|
table_dimensions:
|
||||||
|
type: double_array
|
||||||
|
validation:
|
||||||
|
fixed_size<>: [3]
|
||||||
|
table_pose:
|
||||||
|
type: double_array
|
||||||
|
validation:
|
||||||
|
fixed_size<>: [6]
|
||||||
|
object_name:
|
||||||
|
type: string
|
||||||
|
validation:
|
||||||
|
not_empty<>: []
|
||||||
|
object_reference_frame:
|
||||||
|
type: string
|
||||||
|
validation:
|
||||||
|
not_empty<>: []
|
||||||
|
object_dimensions:
|
||||||
|
type: double_array
|
||||||
|
validation:
|
||||||
|
fixed_size<>: [2]
|
||||||
|
object_pose:
|
||||||
|
type: double_array
|
||||||
|
validation:
|
||||||
|
fixed_size<>: [6]
|
||||||
|
spawn_table:
|
||||||
|
type: bool
|
||||||
|
default_value: true
|
||||||
|
max_solutions:
|
||||||
|
type: int
|
||||||
|
default_value: 10
|
||||||
|
arm_group_name:
|
||||||
|
type: string
|
||||||
|
validation:
|
||||||
|
not_empty<>: []
|
||||||
|
eef_name:
|
||||||
|
type: string
|
||||||
|
validation:
|
||||||
|
not_empty<>: []
|
||||||
|
hand_group_name:
|
||||||
|
type: string
|
||||||
|
validation:
|
||||||
|
not_empty<>: []
|
||||||
|
hand_frame:
|
||||||
|
type: string
|
||||||
|
validation:
|
||||||
|
not_empty<>: []
|
||||||
|
hand_open_pose:
|
||||||
|
type: string
|
||||||
|
validation:
|
||||||
|
not_empty<>: []
|
||||||
|
hand_close_pose:
|
||||||
|
type: string
|
||||||
|
validation:
|
||||||
|
not_empty<>: []
|
||||||
|
arm_home_pose:
|
||||||
|
type: string
|
||||||
|
validation:
|
||||||
|
not_empty<>: []
|
||||||
|
# Scene frames
|
||||||
|
world_frame:
|
||||||
|
type: string
|
||||||
|
validation:
|
||||||
|
not_empty<>: []
|
||||||
|
surface_link:
|
||||||
|
type: string
|
||||||
|
validation:
|
||||||
|
not_empty<>: []
|
||||||
|
grasp_frame_transform:
|
||||||
|
type: double_array
|
||||||
|
validation:
|
||||||
|
fixed_size<>: [6]
|
||||||
|
place_pose:
|
||||||
|
type: double_array
|
||||||
|
validation:
|
||||||
|
fixed_size<>: [6]
|
||||||
|
place_surface_offset:
|
||||||
|
type: double
|
||||||
|
approach_object_min_dist:
|
||||||
|
type: double
|
||||||
|
approach_object_max_dist:
|
||||||
|
type: double
|
||||||
|
lift_object_min_dist:
|
||||||
|
type: double
|
||||||
|
lift_object_max_dist:
|
||||||
|
type: double
|
||||||
@ -34,11 +34,25 @@
|
|||||||
Desc: A demo to show MoveIt Task Constructor in action
|
Desc: A demo to show MoveIt Task Constructor in action
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
#include <Eigen/Geometry>
|
||||||
#include <moveit_task_constructor_demo/pick_place_task.h>
|
#include <moveit_task_constructor_demo/pick_place_task.h>
|
||||||
#include <rosparam_shortcuts/rosparam_shortcuts.h>
|
#include <geometry_msgs/msg/pose.hpp>
|
||||||
|
#include "pick_place_demo_parameters.hpp"
|
||||||
|
|
||||||
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_demo");
|
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_demo");
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
Eigen::Isometry3d vectorToEigen(const std::vector<double>& values) {
|
||||||
|
return Eigen::Translation3d(values[0], values[1], values[2]) *
|
||||||
|
Eigen::AngleAxisd(values[3], Eigen::Vector3d::UnitX()) *
|
||||||
|
Eigen::AngleAxisd(values[4], Eigen::Vector3d::UnitY()) *
|
||||||
|
Eigen::AngleAxisd(values[5], Eigen::Vector3d::UnitZ());
|
||||||
|
}
|
||||||
|
geometry_msgs::msg::Pose vectorToPose(const std::vector<double>& values) {
|
||||||
|
return tf2::toMsg(vectorToEigen(values));
|
||||||
|
};
|
||||||
|
} // namespace
|
||||||
|
|
||||||
namespace moveit_task_constructor_demo {
|
namespace moveit_task_constructor_demo {
|
||||||
|
|
||||||
void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi,
|
void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi,
|
||||||
@ -47,106 +61,46 @@ void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi,
|
|||||||
throw std::runtime_error("Failed to spawn object: " + object.id);
|
throw std::runtime_error("Failed to spawn object: " + object.id);
|
||||||
}
|
}
|
||||||
|
|
||||||
moveit_msgs::msg::CollisionObject createTable(const rclcpp::Node::SharedPtr& node) {
|
moveit_msgs::msg::CollisionObject createTable(const pick_place_task_demo::Params& params) {
|
||||||
std::string table_name, table_reference_frame;
|
geometry_msgs::msg::Pose pose = vectorToPose(params.table_pose);
|
||||||
std::vector<double> table_dimensions;
|
|
||||||
geometry_msgs::msg::Pose pose;
|
|
||||||
std::size_t errors = 0;
|
|
||||||
errors += !rosparam_shortcuts::get(node, "table_name", table_name);
|
|
||||||
errors += !rosparam_shortcuts::get(node, "table_reference_frame", table_reference_frame);
|
|
||||||
errors += !rosparam_shortcuts::get(node, "table_dimensions", table_dimensions);
|
|
||||||
errors += !rosparam_shortcuts::get(node, "table_pose", pose);
|
|
||||||
rosparam_shortcuts::shutdownIfError(errors);
|
|
||||||
|
|
||||||
moveit_msgs::msg::CollisionObject object;
|
moveit_msgs::msg::CollisionObject object;
|
||||||
object.id = table_name;
|
object.id = params.table_name;
|
||||||
object.header.frame_id = table_reference_frame;
|
object.header.frame_id = params.table_reference_frame;
|
||||||
object.primitives.resize(1);
|
object.primitives.resize(1);
|
||||||
object.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX;
|
object.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX;
|
||||||
object.primitives[0].dimensions = { table_dimensions.at(0), table_dimensions.at(1), table_dimensions.at(2) };
|
object.primitives[0].dimensions = { params.table_dimensions.at(0), params.table_dimensions.at(1),
|
||||||
pose.position.z -= 0.5 * table_dimensions[2]; // align surface with world
|
params.table_dimensions.at(2) };
|
||||||
|
pose.position.z -= 0.5 * params.table_dimensions[2]; // align surface with world
|
||||||
object.primitive_poses.push_back(pose);
|
object.primitive_poses.push_back(pose);
|
||||||
return object;
|
return object;
|
||||||
}
|
}
|
||||||
|
|
||||||
moveit_msgs::msg::CollisionObject createObject(const rclcpp::Node::SharedPtr& node) {
|
moveit_msgs::msg::CollisionObject createObject(const pick_place_task_demo::Params& params) {
|
||||||
std::string object_name, object_reference_frame;
|
geometry_msgs::msg::Pose pose = vectorToPose(params.object_pose);
|
||||||
std::vector<double> object_dimensions;
|
|
||||||
geometry_msgs::msg::Pose pose;
|
|
||||||
std::size_t error = 0;
|
|
||||||
error += !rosparam_shortcuts::get(node, "object_name", object_name);
|
|
||||||
error += !rosparam_shortcuts::get(node, "object_reference_frame", object_reference_frame);
|
|
||||||
error += !rosparam_shortcuts::get(node, "object_dimensions", object_dimensions);
|
|
||||||
error += !rosparam_shortcuts::get(node, "object_pose", pose);
|
|
||||||
rosparam_shortcuts::shutdownIfError(error);
|
|
||||||
|
|
||||||
moveit_msgs::msg::CollisionObject object;
|
moveit_msgs::msg::CollisionObject object;
|
||||||
object.id = object_name;
|
object.id = params.object_name;
|
||||||
object.header.frame_id = object_reference_frame;
|
object.header.frame_id = params.object_reference_frame;
|
||||||
object.primitives.resize(1);
|
object.primitives.resize(1);
|
||||||
object.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER;
|
object.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER;
|
||||||
object.primitives[0].dimensions = { object_dimensions.at(0), object_dimensions.at(1) };
|
object.primitives[0].dimensions = { params.object_dimensions.at(0), params.object_dimensions.at(1) };
|
||||||
pose.position.z += 0.5 * object_dimensions[0];
|
pose.position.z += 0.5 * params.object_dimensions[0];
|
||||||
object.primitive_poses.push_back(pose);
|
object.primitive_poses.push_back(pose);
|
||||||
return object;
|
return object;
|
||||||
}
|
}
|
||||||
|
|
||||||
void setupDemoScene(const rclcpp::Node::SharedPtr& node) {
|
void setupDemoScene(const pick_place_task_demo::Params& params) {
|
||||||
// Add table and object to planning scene
|
// Add table and object to planning scene
|
||||||
rclcpp::sleep_for(std::chrono::microseconds(100)); // Wait for ApplyPlanningScene service
|
rclcpp::sleep_for(std::chrono::microseconds(100)); // Wait for ApplyPlanningScene service
|
||||||
moveit::planning_interface::PlanningSceneInterface psi;
|
moveit::planning_interface::PlanningSceneInterface psi;
|
||||||
if (bool spawn_table = true; rosparam_shortcuts::get(node, "spawn_table", spawn_table) && spawn_table)
|
if (params.spawn_table)
|
||||||
spawnObject(psi, createTable(node));
|
spawnObject(psi, createTable(params));
|
||||||
spawnObject(psi, createObject(node));
|
spawnObject(psi, createObject(params));
|
||||||
}
|
}
|
||||||
|
|
||||||
PickPlaceTask::PickPlaceTask(const std::string& task_name, const rclcpp::Node::SharedPtr& node)
|
PickPlaceTask::PickPlaceTask(const std::string& task_name) : task_name_(task_name) {}
|
||||||
: node_(node), task_name_(task_name) {
|
|
||||||
loadParameters();
|
|
||||||
}
|
|
||||||
|
|
||||||
void PickPlaceTask::loadParameters() {
|
bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_task_demo::Params& params) {
|
||||||
/****************************************************
|
|
||||||
* *
|
|
||||||
* Load Parameters *
|
|
||||||
* *
|
|
||||||
***************************************************/
|
|
||||||
RCLCPP_INFO(LOGGER, "Loading task parameters");
|
|
||||||
|
|
||||||
// Planning group properties
|
|
||||||
size_t errors = 0;
|
|
||||||
errors += !rosparam_shortcuts::get(node_, "arm_group_name", arm_group_name_);
|
|
||||||
errors += !rosparam_shortcuts::get(node_, "hand_group_name", hand_group_name_);
|
|
||||||
errors += !rosparam_shortcuts::get(node_, "eef_name", eef_name_);
|
|
||||||
errors += !rosparam_shortcuts::get(node_, "hand_frame", hand_frame_);
|
|
||||||
errors += !rosparam_shortcuts::get(node_, "world_frame", world_frame_);
|
|
||||||
errors += !rosparam_shortcuts::get(node_, "grasp_frame_transform", grasp_frame_transform_);
|
|
||||||
|
|
||||||
// Predefined pose targets
|
|
||||||
errors += !rosparam_shortcuts::get(node_, "hand_open_pose", hand_open_pose_);
|
|
||||||
errors += !rosparam_shortcuts::get(node_, "hand_close_pose", hand_close_pose_);
|
|
||||||
errors += !rosparam_shortcuts::get(node_, "arm_home_pose", arm_home_pose_);
|
|
||||||
|
|
||||||
// Target object
|
|
||||||
errors += !rosparam_shortcuts::get(node_, "object_name", object_name_);
|
|
||||||
errors += !rosparam_shortcuts::get(node_, "object_dimensions", object_dimensions_);
|
|
||||||
errors += !rosparam_shortcuts::get(node_, "object_reference_frame", object_reference_frame_);
|
|
||||||
errors += !rosparam_shortcuts::get(node_, "surface_link", surface_link_);
|
|
||||||
support_surfaces_ = { surface_link_ };
|
|
||||||
|
|
||||||
// Pick/Place metrics
|
|
||||||
errors += !rosparam_shortcuts::get(node_, "approach_object_min_dist", approach_object_min_dist_);
|
|
||||||
errors += !rosparam_shortcuts::get(node_, "approach_object_max_dist", approach_object_max_dist_);
|
|
||||||
errors += !rosparam_shortcuts::get(node_, "lift_object_min_dist", lift_object_min_dist_);
|
|
||||||
errors += !rosparam_shortcuts::get(node_, "lift_object_max_dist", lift_object_max_dist_);
|
|
||||||
errors += !rosparam_shortcuts::get(node_, "place_surface_offset", place_surface_offset_);
|
|
||||||
errors += !rosparam_shortcuts::get(node_, "place_pose", place_pose_);
|
|
||||||
rosparam_shortcuts::shutdownIfError(errors);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool PickPlaceTask::init() {
|
|
||||||
RCLCPP_INFO(LOGGER, "Initializing task pipeline");
|
RCLCPP_INFO(LOGGER, "Initializing task pipeline");
|
||||||
const std::string object = object_name_;
|
|
||||||
|
|
||||||
// Reset ROS introspection before constructing the new object
|
// Reset ROS introspection before constructing the new object
|
||||||
// TODO(v4hn): global storage for Introspection services to enable one-liner
|
// TODO(v4hn): global storage for Introspection services to enable one-liner
|
||||||
@ -155,10 +109,10 @@ bool PickPlaceTask::init() {
|
|||||||
|
|
||||||
Task& t = *task_;
|
Task& t = *task_;
|
||||||
t.stages()->setName(task_name_);
|
t.stages()->setName(task_name_);
|
||||||
t.loadRobotModel(node_);
|
t.loadRobotModel(node);
|
||||||
|
|
||||||
// Sampling planner
|
// Sampling planner
|
||||||
auto sampling_planner = std::make_shared<solvers::PipelinePlanner>(node_);
|
auto sampling_planner = std::make_shared<solvers::PipelinePlanner>(node);
|
||||||
sampling_planner->setProperty("goal_joint_tolerance", 1e-5);
|
sampling_planner->setProperty("goal_joint_tolerance", 1e-5);
|
||||||
|
|
||||||
// Cartesian planner
|
// Cartesian planner
|
||||||
@ -168,11 +122,11 @@ bool PickPlaceTask::init() {
|
|||||||
cartesian_planner->setStepSize(.01);
|
cartesian_planner->setStepSize(.01);
|
||||||
|
|
||||||
// Set task properties
|
// Set task properties
|
||||||
t.setProperty("group", arm_group_name_);
|
t.setProperty("group", params.arm_group_name);
|
||||||
t.setProperty("eef", eef_name_);
|
t.setProperty("eef", params.eef_name);
|
||||||
t.setProperty("hand", hand_group_name_);
|
t.setProperty("hand", params.hand_group_name);
|
||||||
t.setProperty("hand_grasping_frame", hand_frame_);
|
t.setProperty("hand_grasping_frame", params.hand_frame);
|
||||||
t.setProperty("ik_frame", hand_frame_);
|
t.setProperty("ik_frame", params.hand_frame);
|
||||||
|
|
||||||
/****************************************************
|
/****************************************************
|
||||||
* *
|
* *
|
||||||
@ -186,7 +140,7 @@ bool PickPlaceTask::init() {
|
|||||||
// Verify that object is not attached
|
// Verify that object is not attached
|
||||||
auto applicability_filter =
|
auto applicability_filter =
|
||||||
std::make_unique<stages::PredicateFilter>("applicability test", std::move(current_state));
|
std::make_unique<stages::PredicateFilter>("applicability test", std::move(current_state));
|
||||||
applicability_filter->setPredicate([object](const SolutionBase& s, std::string& comment) {
|
applicability_filter->setPredicate([object = params.object_name](const SolutionBase& s, std::string& comment) {
|
||||||
if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) {
|
if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) {
|
||||||
comment = "object with id '" + object + "' is already attached and cannot be picked";
|
comment = "object with id '" + object + "' is already attached and cannot be picked";
|
||||||
return false;
|
return false;
|
||||||
@ -205,8 +159,8 @@ bool PickPlaceTask::init() {
|
|||||||
***************************************************/
|
***************************************************/
|
||||||
{ // Open Hand
|
{ // Open Hand
|
||||||
auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
|
auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
|
||||||
stage->setGroup(hand_group_name_);
|
stage->setGroup(params.hand_group_name);
|
||||||
stage->setGoal(hand_open_pose_);
|
stage->setGoal(params.hand_open_pose);
|
||||||
t.add(std::move(stage));
|
t.add(std::move(stage));
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -217,7 +171,7 @@ bool PickPlaceTask::init() {
|
|||||||
***************************************************/
|
***************************************************/
|
||||||
{ // Move-to pre-grasp
|
{ // Move-to pre-grasp
|
||||||
auto stage = std::make_unique<stages::Connect>(
|
auto stage = std::make_unique<stages::Connect>(
|
||||||
"move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } });
|
"move to pick", stages::Connect::GroupPlannerVector{ { params.arm_group_name, sampling_planner } });
|
||||||
stage->setTimeout(5.0);
|
stage->setTimeout(5.0);
|
||||||
stage->properties().configureInitFrom(Stage::PARENT);
|
stage->properties().configureInitFrom(Stage::PARENT);
|
||||||
t.add(std::move(stage));
|
t.add(std::move(stage));
|
||||||
@ -240,13 +194,13 @@ bool PickPlaceTask::init() {
|
|||||||
{
|
{
|
||||||
auto stage = std::make_unique<stages::MoveRelative>("approach object", cartesian_planner);
|
auto stage = std::make_unique<stages::MoveRelative>("approach object", cartesian_planner);
|
||||||
stage->properties().set("marker_ns", "approach_object");
|
stage->properties().set("marker_ns", "approach_object");
|
||||||
stage->properties().set("link", hand_frame_);
|
stage->properties().set("link", params.hand_frame);
|
||||||
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
|
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
|
||||||
stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_);
|
stage->setMinMaxDistance(params.approach_object_min_dist, params.approach_object_max_dist);
|
||||||
|
|
||||||
// Set hand forward direction
|
// Set hand forward direction
|
||||||
geometry_msgs::msg::Vector3Stamped vec;
|
geometry_msgs::msg::Vector3Stamped vec;
|
||||||
vec.header.frame_id = hand_frame_;
|
vec.header.frame_id = params.hand_frame;
|
||||||
vec.vector.z = 1.0;
|
vec.vector.z = 1.0;
|
||||||
stage->setDirection(vec);
|
stage->setDirection(vec);
|
||||||
grasp->insert(std::move(stage));
|
grasp->insert(std::move(stage));
|
||||||
@ -260,8 +214,8 @@ bool PickPlaceTask::init() {
|
|||||||
auto stage = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
|
auto stage = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
|
||||||
stage->properties().configureInitFrom(Stage::PARENT);
|
stage->properties().configureInitFrom(Stage::PARENT);
|
||||||
stage->properties().set("marker_ns", "grasp_pose");
|
stage->properties().set("marker_ns", "grasp_pose");
|
||||||
stage->setPreGraspPose(hand_open_pose_);
|
stage->setPreGraspPose(params.hand_open_pose);
|
||||||
stage->setObject(object);
|
stage->setObject(params.object_name);
|
||||||
stage->setAngleDelta(M_PI / 12);
|
stage->setAngleDelta(M_PI / 12);
|
||||||
stage->setMonitoredStage(current_state_ptr); // Hook into current state
|
stage->setMonitoredStage(current_state_ptr); // Hook into current state
|
||||||
|
|
||||||
@ -269,7 +223,7 @@ bool PickPlaceTask::init() {
|
|||||||
auto wrapper = std::make_unique<stages::ComputeIK>("grasp pose IK", std::move(stage));
|
auto wrapper = std::make_unique<stages::ComputeIK>("grasp pose IK", std::move(stage));
|
||||||
wrapper->setMaxIKSolutions(8);
|
wrapper->setMaxIKSolutions(8);
|
||||||
wrapper->setMinSolutionDistance(1.0);
|
wrapper->setMinSolutionDistance(1.0);
|
||||||
wrapper->setIKFrame(grasp_frame_transform_, hand_frame_);
|
wrapper->setIKFrame(vectorToEigen(params.grasp_frame_transform), params.hand_frame);
|
||||||
wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" });
|
wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" });
|
||||||
wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" });
|
wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" });
|
||||||
grasp->insert(std::move(wrapper));
|
grasp->insert(std::move(wrapper));
|
||||||
@ -281,7 +235,8 @@ bool PickPlaceTask::init() {
|
|||||||
{
|
{
|
||||||
auto stage = std::make_unique<stages::ModifyPlanningScene>("allow collision (hand,object)");
|
auto stage = std::make_unique<stages::ModifyPlanningScene>("allow collision (hand,object)");
|
||||||
stage->allowCollisions(
|
stage->allowCollisions(
|
||||||
object, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(),
|
params.object_name,
|
||||||
|
t.getRobotModel()->getJointModelGroup(params.hand_group_name)->getLinkModelNamesWithCollisionGeometry(),
|
||||||
true);
|
true);
|
||||||
grasp->insert(std::move(stage));
|
grasp->insert(std::move(stage));
|
||||||
}
|
}
|
||||||
@ -291,8 +246,8 @@ bool PickPlaceTask::init() {
|
|||||||
***************************************************/
|
***************************************************/
|
||||||
{
|
{
|
||||||
auto stage = std::make_unique<stages::MoveTo>("close hand", sampling_planner);
|
auto stage = std::make_unique<stages::MoveTo>("close hand", sampling_planner);
|
||||||
stage->setGroup(hand_group_name_);
|
stage->setGroup(params.hand_group_name);
|
||||||
stage->setGoal(hand_close_pose_);
|
stage->setGoal(params.hand_close_pose);
|
||||||
grasp->insert(std::move(stage));
|
grasp->insert(std::move(stage));
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -301,7 +256,7 @@ bool PickPlaceTask::init() {
|
|||||||
***************************************************/
|
***************************************************/
|
||||||
{
|
{
|
||||||
auto stage = std::make_unique<stages::ModifyPlanningScene>("attach object");
|
auto stage = std::make_unique<stages::ModifyPlanningScene>("attach object");
|
||||||
stage->attachObject(object, hand_frame_);
|
stage->attachObject(params.object_name, params.hand_frame);
|
||||||
attach_object_stage = stage.get();
|
attach_object_stage = stage.get();
|
||||||
grasp->insert(std::move(stage));
|
grasp->insert(std::move(stage));
|
||||||
}
|
}
|
||||||
@ -311,7 +266,7 @@ bool PickPlaceTask::init() {
|
|||||||
***************************************************/
|
***************************************************/
|
||||||
{
|
{
|
||||||
auto stage = std::make_unique<stages::ModifyPlanningScene>("allow collision (object,support)");
|
auto stage = std::make_unique<stages::ModifyPlanningScene>("allow collision (object,support)");
|
||||||
stage->allowCollisions({ object }, support_surfaces_, true);
|
stage->allowCollisions({ params.object_name }, { params.surface_link }, true);
|
||||||
grasp->insert(std::move(stage));
|
grasp->insert(std::move(stage));
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -321,13 +276,13 @@ bool PickPlaceTask::init() {
|
|||||||
{
|
{
|
||||||
auto stage = std::make_unique<stages::MoveRelative>("lift object", cartesian_planner);
|
auto stage = std::make_unique<stages::MoveRelative>("lift object", cartesian_planner);
|
||||||
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
|
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
|
||||||
stage->setMinMaxDistance(lift_object_min_dist_, lift_object_max_dist_);
|
stage->setMinMaxDistance(params.lift_object_min_dist, params.lift_object_max_dist);
|
||||||
stage->setIKFrame(hand_frame_);
|
stage->setIKFrame(params.hand_frame);
|
||||||
stage->properties().set("marker_ns", "lift_object");
|
stage->properties().set("marker_ns", "lift_object");
|
||||||
|
|
||||||
// Set upward direction
|
// Set upward direction
|
||||||
geometry_msgs::msg::Vector3Stamped vec;
|
geometry_msgs::msg::Vector3Stamped vec;
|
||||||
vec.header.frame_id = world_frame_;
|
vec.header.frame_id = params.world_frame;
|
||||||
vec.vector.z = 1.0;
|
vec.vector.z = 1.0;
|
||||||
stage->setDirection(vec);
|
stage->setDirection(vec);
|
||||||
grasp->insert(std::move(stage));
|
grasp->insert(std::move(stage));
|
||||||
@ -338,7 +293,7 @@ bool PickPlaceTask::init() {
|
|||||||
***************************************************/
|
***************************************************/
|
||||||
{
|
{
|
||||||
auto stage = std::make_unique<stages::ModifyPlanningScene>("forbid collision (object,surface)");
|
auto stage = std::make_unique<stages::ModifyPlanningScene>("forbid collision (object,surface)");
|
||||||
stage->allowCollisions({ object }, support_surfaces_, false);
|
stage->allowCollisions({ params.object_name }, { params.surface_link }, false);
|
||||||
grasp->insert(std::move(stage));
|
grasp->insert(std::move(stage));
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -353,7 +308,7 @@ bool PickPlaceTask::init() {
|
|||||||
*****************************************************/
|
*****************************************************/
|
||||||
{
|
{
|
||||||
auto stage = std::make_unique<stages::Connect>(
|
auto stage = std::make_unique<stages::Connect>(
|
||||||
"move to place", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } });
|
"move to place", stages::Connect::GroupPlannerVector{ { params.arm_group_name, sampling_planner } });
|
||||||
stage->setTimeout(5.0);
|
stage->setTimeout(5.0);
|
||||||
stage->properties().configureInitFrom(Stage::PARENT);
|
stage->properties().configureInitFrom(Stage::PARENT);
|
||||||
t.add(std::move(stage));
|
t.add(std::move(stage));
|
||||||
@ -375,13 +330,13 @@ bool PickPlaceTask::init() {
|
|||||||
{
|
{
|
||||||
auto stage = std::make_unique<stages::MoveRelative>("lower object", cartesian_planner);
|
auto stage = std::make_unique<stages::MoveRelative>("lower object", cartesian_planner);
|
||||||
stage->properties().set("marker_ns", "lower_object");
|
stage->properties().set("marker_ns", "lower_object");
|
||||||
stage->properties().set("link", hand_frame_);
|
stage->properties().set("link", params.hand_frame);
|
||||||
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
|
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
|
||||||
stage->setMinMaxDistance(.03, .13);
|
stage->setMinMaxDistance(.03, .13);
|
||||||
|
|
||||||
// Set downward direction
|
// Set downward direction
|
||||||
geometry_msgs::msg::Vector3Stamped vec;
|
geometry_msgs::msg::Vector3Stamped vec;
|
||||||
vec.header.frame_id = world_frame_;
|
vec.header.frame_id = params.world_frame;
|
||||||
vec.vector.z = -1.0;
|
vec.vector.z = -1.0;
|
||||||
stage->setDirection(vec);
|
stage->setDirection(vec);
|
||||||
place->insert(std::move(stage));
|
place->insert(std::move(stage));
|
||||||
@ -395,20 +350,20 @@ bool PickPlaceTask::init() {
|
|||||||
auto stage = std::make_unique<stages::GeneratePlacePose>("generate place pose");
|
auto stage = std::make_unique<stages::GeneratePlacePose>("generate place pose");
|
||||||
stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" });
|
stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" });
|
||||||
stage->properties().set("marker_ns", "place_pose");
|
stage->properties().set("marker_ns", "place_pose");
|
||||||
stage->setObject(object);
|
stage->setObject(params.object_name);
|
||||||
|
|
||||||
// Set target pose
|
// Set target pose
|
||||||
geometry_msgs::msg::PoseStamped p;
|
geometry_msgs::msg::PoseStamped p;
|
||||||
p.header.frame_id = object_reference_frame_;
|
p.header.frame_id = params.object_reference_frame;
|
||||||
p.pose = place_pose_;
|
p.pose = vectorToPose(params.place_pose);
|
||||||
p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_;
|
p.pose.position.z += 0.5 * params.object_dimensions[0] + params.place_surface_offset;
|
||||||
stage->setPose(p);
|
stage->setPose(p);
|
||||||
stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage
|
stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage
|
||||||
|
|
||||||
// Compute IK
|
// Compute IK
|
||||||
auto wrapper = std::make_unique<stages::ComputeIK>("place pose IK", std::move(stage));
|
auto wrapper = std::make_unique<stages::ComputeIK>("place pose IK", std::move(stage));
|
||||||
wrapper->setMaxIKSolutions(2);
|
wrapper->setMaxIKSolutions(2);
|
||||||
wrapper->setIKFrame(grasp_frame_transform_, hand_frame_);
|
wrapper->setIKFrame(vectorToEigen(params.grasp_frame_transform), params.hand_frame);
|
||||||
wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" });
|
wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" });
|
||||||
wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" });
|
wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" });
|
||||||
place->insert(std::move(wrapper));
|
place->insert(std::move(wrapper));
|
||||||
@ -419,8 +374,8 @@ bool PickPlaceTask::init() {
|
|||||||
*****************************************************/
|
*****************************************************/
|
||||||
{
|
{
|
||||||
auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
|
auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
|
||||||
stage->setGroup(hand_group_name_);
|
stage->setGroup(params.hand_group_name);
|
||||||
stage->setGoal(hand_open_pose_);
|
stage->setGoal(params.hand_open_pose);
|
||||||
place->insert(std::move(stage));
|
place->insert(std::move(stage));
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -429,7 +384,8 @@ bool PickPlaceTask::init() {
|
|||||||
*****************************************************/
|
*****************************************************/
|
||||||
{
|
{
|
||||||
auto stage = std::make_unique<stages::ModifyPlanningScene>("forbid collision (hand,object)");
|
auto stage = std::make_unique<stages::ModifyPlanningScene>("forbid collision (hand,object)");
|
||||||
stage->allowCollisions(object_name_, *t.getRobotModel()->getJointModelGroup(hand_group_name_), false);
|
stage->allowCollisions(params.object_name, *t.getRobotModel()->getJointModelGroup(params.hand_group_name),
|
||||||
|
false);
|
||||||
place->insert(std::move(stage));
|
place->insert(std::move(stage));
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -438,7 +394,7 @@ bool PickPlaceTask::init() {
|
|||||||
*****************************************************/
|
*****************************************************/
|
||||||
{
|
{
|
||||||
auto stage = std::make_unique<stages::ModifyPlanningScene>("detach object");
|
auto stage = std::make_unique<stages::ModifyPlanningScene>("detach object");
|
||||||
stage->detachObject(object_name_, hand_frame_);
|
stage->detachObject(params.object_name, params.hand_frame);
|
||||||
place->insert(std::move(stage));
|
place->insert(std::move(stage));
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -449,10 +405,10 @@ bool PickPlaceTask::init() {
|
|||||||
auto stage = std::make_unique<stages::MoveRelative>("retreat after place", cartesian_planner);
|
auto stage = std::make_unique<stages::MoveRelative>("retreat after place", cartesian_planner);
|
||||||
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
|
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
|
||||||
stage->setMinMaxDistance(.12, .25);
|
stage->setMinMaxDistance(.12, .25);
|
||||||
stage->setIKFrame(hand_frame_);
|
stage->setIKFrame(params.hand_frame);
|
||||||
stage->properties().set("marker_ns", "retreat");
|
stage->properties().set("marker_ns", "retreat");
|
||||||
geometry_msgs::msg::Vector3Stamped vec;
|
geometry_msgs::msg::Vector3Stamped vec;
|
||||||
vec.header.frame_id = hand_frame_;
|
vec.header.frame_id = params.hand_frame;
|
||||||
vec.vector.z = -1.0;
|
vec.vector.z = -1.0;
|
||||||
stage->setDirection(vec);
|
stage->setDirection(vec);
|
||||||
place->insert(std::move(stage));
|
place->insert(std::move(stage));
|
||||||
@ -470,7 +426,7 @@ bool PickPlaceTask::init() {
|
|||||||
{
|
{
|
||||||
auto stage = std::make_unique<stages::MoveTo>("move home", sampling_planner);
|
auto stage = std::make_unique<stages::MoveTo>("move home", sampling_planner);
|
||||||
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
|
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
|
||||||
stage->setGoal(arm_home_pose_);
|
stage->setGoal(params.arm_home_pose);
|
||||||
stage->restrictDirection(stages::MoveTo::FORWARD);
|
stage->restrictDirection(stages::MoveTo::FORWARD);
|
||||||
t.add(std::move(stage));
|
t.add(std::move(stage));
|
||||||
}
|
}
|
||||||
@ -486,10 +442,8 @@ bool PickPlaceTask::init() {
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool PickPlaceTask::plan() {
|
bool PickPlaceTask::plan(const std::size_t max_solutions) {
|
||||||
RCLCPP_INFO(LOGGER, "Start searching for task solutions");
|
RCLCPP_INFO(LOGGER, "Start searching for task solutions");
|
||||||
int max_solutions = 10;
|
|
||||||
rosparam_shortcuts::get(node_, "max_solutions", max_solutions);
|
|
||||||
|
|
||||||
return static_cast<bool>(task_->plan(max_solutions));
|
return static_cast<bool>(task_->plan(max_solutions));
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user