diff --git a/moveit_reach_plugins/CMakeLists.txt b/moveit_reach_plugins/CMakeLists.txt index d494e7ec..ea80e254 100644 --- a/moveit_reach_plugins/CMakeLists.txt +++ b/moveit_reach_plugins/CMakeLists.txt @@ -76,8 +76,8 @@ pluginlib_export_plugin_description_file(reach_core eval_plugin_description.xml) add_library(ik_solver_plugins src/ik/moveit_ik_solver.cpp src/ik/discretized_moveit_ik_solver.cpp - src/ik/cartesian_retrieval_ik_solver.cpp - src/ik/planner_based_ik_solver.cpp +# src/ik/cartesian_retrieval_ik_solver.cpp +# src/ik/planner_based_ik_solver.cpp ) target_include_directories(ik_solver_plugins PUBLIC @@ -90,7 +90,6 @@ target_link_libraries(ik_solver_plugins ament_target_dependencies(ik_solver_plugins moveit_ros_planning_interface geometric_shapes -# reach_core pluginlib rclcpp tf2_eigen @@ -114,6 +113,27 @@ target_link_libraries(reach_display_plugins ) pluginlib_export_plugin_description_file(reach_core display_plugin_description.xml) +# MoveIt Reach Path Plugin +add_library(path_plugins + src/path/cartesian_path_generation.cpp +) +target_include_directories(path_plugins + PUBLIC + $ + $ +) +target_link_libraries(path_plugins + ${PROJECT_NAME}_utils +) +ament_target_dependencies(path_plugins + moveit_ros_planning_interface + geometric_shapes + pluginlib + rclcpp + tf2_eigen +) +pluginlib_export_plugin_description_file(reach_core path_plugin_description.xml) + # IK Plugin Test #add_executable(ik_plugin_test # test/plugin_test_node.cpp @@ -136,6 +156,7 @@ install( evaluation_plugins ik_solver_plugins reach_display_plugins + path_plugins EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -151,6 +172,7 @@ ament_export_libraries( evaluation_plugins ik_solver_plugins reach_display_plugins + path_plugins ) ament_export_include_directories( diff --git a/moveit_reach_plugins/ik_plugin_description.xml b/moveit_reach_plugins/ik_plugin_description.xml index 2a4a8aee..d85ee108 100644 --- a/moveit_reach_plugins/ik_plugin_description.xml +++ b/moveit_reach_plugins/ik_plugin_description.xml @@ -13,16 +13,16 @@ This plugin discretizes the target pose around the Z-axis and outputs the solution with the highest score - - - An inverse kinematics solver plugin which utilizes the MoveIt framework for solving robot inverse kinematics with respect to a given planning environment - This plugin additionally does retrieval path computation from the specified pose. - - - - - An inverse kinematics solver plugin which utilizes the MoveIt framework for solving robot inverse kinematics with respect to a given planning environment - This plugin additionally does path planning to the specified pose. - - + + + + + + + + + + + + diff --git a/moveit_reach_plugins/include/moveit_reach_plugins/ik/discretized_moveit_ik_solver.h b/moveit_reach_plugins/include/moveit_reach_plugins/ik/discretized_moveit_ik_solver.h index 694bfe90..c9471d88 100644 --- a/moveit_reach_plugins/include/moveit_reach_plugins/ik/discretized_moveit_ik_solver.h +++ b/moveit_reach_plugins/include/moveit_reach_plugins/ik/discretized_moveit_ik_solver.h @@ -34,10 +34,8 @@ class DiscretizedMoveItIKSolver : public MoveItIKSolver { virtual std::optional solveIKFromSeed( const Eigen::Isometry3d& target, - const std::map& seed, std::vector& solution, - std::vector& joint_space_trajectory, - std::vector& cartesian_space_waypoints, double& fraction, - moveit_msgs::msg::RobotTrajectory& moveit_trajectory) override; + const std::map& seed, + std::vector& solution) override; protected: double dt_; diff --git a/moveit_reach_plugins/include/moveit_reach_plugins/ik/moveit_ik_solver.h b/moveit_reach_plugins/include/moveit_reach_plugins/ik/moveit_ik_solver.h index 04781b39..fe1ad74f 100644 --- a/moveit_reach_plugins/include/moveit_reach_plugins/ik/moveit_ik_solver.h +++ b/moveit_reach_plugins/include/moveit_reach_plugins/ik/moveit_ik_solver.h @@ -23,17 +23,6 @@ // PlanningScene #include -// namespace moveit -//{ -// namespace core -//{ -// class RobotModel; -// typedef std::shared_ptr RobotModelConstPtr; -// class JointModelGroup; -// class RobotState; -//} -//} - namespace planning_scene { class PlanningScene; typedef std::shared_ptr PlanningScenePtr; @@ -63,10 +52,8 @@ class MoveItIKSolver : public reach::plugins::IKSolverBase { virtual std::optional solveIKFromSeed( const Eigen::Isometry3d& target, - const std::map& seed, std::vector& solution, - std::vector& joint_space_trajectory, - std::vector& cartesian_space_waypoints, double& fraction, - moveit_msgs::msg::RobotTrajectory& moveit_trajectory) override; + const std::map& seed, + std::vector& solution) override; virtual std::vector getJointNames() const override; diff --git a/moveit_reach_plugins/include/moveit_reach_plugins/path/cartesian_path_generation.h b/moveit_reach_plugins/include/moveit_reach_plugins/path/cartesian_path_generation.h new file mode 100644 index 00000000..f166ee78 --- /dev/null +++ b/moveit_reach_plugins/include/moveit_reach_plugins/path/cartesian_path_generation.h @@ -0,0 +1,90 @@ +/* + * Copyright 2022 PickNik, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +/* Authors: Lovro Ivanov, @livanov93 + Desc: +*/ +#ifndef MOVEIT_REACH_PLUGINS_PATH_CARTESIAN_PATH_GENERATION_H +#define MOVEIT_REACH_PLUGINS_PATH_CARTESIAN_PATH_GENERATION_H + +#include +#include + +// PlanningScene +#include + +// cartesian interpolator include +#include "tf2_eigen/tf2_eigen.h" + +#include +#include + +namespace planning_scene { +class PlanningScene; +typedef std::shared_ptr PlanningScenePtr; +} // namespace planning_scene + +namespace moveit_reach_plugins { + +namespace path { + +class CartesianPathGeneration : public reach::plugins::PathBase { + public: + CartesianPathGeneration(); + + ~CartesianPathGeneration() {} + + virtual bool initialize( + const std::string& name, rclcpp::Node::SharedPtr node, + const std::shared_ptr model) override; + + virtual std::optional solvePath( + const std::map& start_state, + std::map& end_state, double& fraction, + moveit_msgs::msg::RobotTrajectory& moveit_trajectory) override; + + virtual std::vector getJointNames() const override; + + bool isIKSolutionValid(moveit::core::RobotState* state, + const moveit::core::JointModelGroup* jmg, + const double* ik_solution) const; + + protected: + moveit::core::RobotModelConstPtr model_; + planning_scene::PlanningScenePtr scene_; + const moveit::core::JointModelGroup* jmg_; + std::string name_; + + // distance to retrieve from ik solution in [m] + double retrieval_path_length_; + double jump_threshold_; + double max_eef_step_; + std::string tool_frame_; + double max_velocity_scaling_factor_; + double max_acceleration_scaling_factor_; + std::string collision_mesh_package_; + std::string collision_mesh_frame_; + // collision checking + double distance_threshold_; + + // transformations + Eigen::Isometry3d global_transformation_; + Eigen::Isometry3d tool_transformation_; +}; + +} // namespace path +} // namespace moveit_reach_plugins + +#endif // MOVEIT_REACH_PLUGINS_PATH_CARTESIAN_PATH_GENERATION_H diff --git a/moveit_reach_plugins/path_plugin_description.xml b/moveit_reach_plugins/path_plugin_description.xml new file mode 100644 index 00000000..bf26a1ba --- /dev/null +++ b/moveit_reach_plugins/path_plugin_description.xml @@ -0,0 +1,9 @@ + + + + + + Cartesian Interpolator plugin + + + diff --git a/moveit_reach_plugins/src/ik/discretized_moveit_ik_solver.cpp b/moveit_reach_plugins/src/ik/discretized_moveit_ik_solver.cpp index e3d225b0..412097a6 100644 --- a/moveit_reach_plugins/src/ik/discretized_moveit_ik_solver.cpp +++ b/moveit_reach_plugins/src/ik/discretized_moveit_ik_solver.cpp @@ -64,9 +64,7 @@ bool DiscretizedMoveItIKSolver::initialize( std::optional DiscretizedMoveItIKSolver::solveIKFromSeed( const Eigen::Isometry3d& target, const std::map& seed, - std::vector& solution, std::vector& joint_space_trajectory, - std::vector& cartesian_space_waypoints, double& fraction, - moveit_msgs::msg::RobotTrajectory& moveit_trajectory) { + std::vector& solution) { // Calculate the number of discretizations necessary to achieve discretization // angle const static int n_discretizations = int((2.0 * M_PI) / dt_); @@ -79,9 +77,8 @@ std::optional DiscretizedMoveItIKSolver::solveIKFromSeed( Eigen::Isometry3d discretized_target( target * Eigen::AngleAxisd(double(i) * dt_, Eigen::Vector3d::UnitZ())); std::vector tmp_solution; - std::optional score = MoveItIKSolver::solveIKFromSeed( - discretized_target, seed, tmp_solution, joint_space_trajectory, - cartesian_space_waypoints, fraction, moveit_trajectory); + std::optional score = + MoveItIKSolver::solveIKFromSeed(discretized_target, seed, tmp_solution); if (score.has_value() && (score.value() > best_score)) { best_score = score.value(); best_solution = std::move(tmp_solution); @@ -94,7 +91,6 @@ std::optional DiscretizedMoveItIKSolver::solveIKFromSeed( solution = std::move(best_solution); return std::optional(best_score); } else { - fraction = 0.0; return {}; } } diff --git a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp index 9b141149..86a0f091 100644 --- a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp +++ b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp @@ -76,8 +76,6 @@ bool MoveItIKSolver::initialize( return false; } - // model_ = moveit::planning_interface::getSharedRobotModelLoader(node, - // "robot_description")->getModel(); model_ = model; if (!model_) { @@ -123,9 +121,7 @@ bool MoveItIKSolver::initialize( std::optional MoveItIKSolver::solveIKFromSeed( const Eigen::Isometry3d& target, const std::map& seed, - std::vector& solution, std::vector& joint_space_trajectory, - std::vector& cartesian_space_waypoints, double& fraction, - moveit_msgs::msg::RobotTrajectory& moveit_trajectory) { + std::vector& solution) { moveit::core::RobotState state(model_); const std::vector& joint_names = @@ -149,7 +145,6 @@ std::optional MoveItIKSolver::solveIKFromSeed( std::bind(&MoveItIKSolver::isIKSolutionValid, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3))) { - fraction = 1.0; solution.clear(); state.copyJointGroupPositions(jmg_, solution); @@ -161,7 +156,6 @@ std::optional MoveItIKSolver::solveIKFromSeed( return eval_->calculateScore(solution_map); } else { - fraction = 0.0; return {}; } } @@ -178,15 +172,6 @@ bool MoveItIKSolver::isIKSolutionValid(moveit::core::RobotState* state, (scene_->distanceToCollision( *state, scene_->getAllowedCollisionMatrix()) < distance_threshold_); - // visualization part for debugging purposes only - /* - if (!colliding && !too_close){ - scene_->setCurrentState(*state); - moveit_msgs::msg::PlanningScene scene_msg; - scene_->getPlanningSceneMsg(scene_msg); - scene_pub_->publish(scene_msg); - } - */ return (!colliding && !too_close); } diff --git a/moveit_reach_plugins/src/path/cartesian_path_generation.cpp b/moveit_reach_plugins/src/path/cartesian_path_generation.cpp new file mode 100644 index 00000000..6cc59e31 --- /dev/null +++ b/moveit_reach_plugins/src/path/cartesian_path_generation.cpp @@ -0,0 +1,350 @@ +/* + * Copyright 2022 PickNik, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +/* Authors: Lovro Ivanov, @livanov93 + Desc: +*/ +#include "moveit_reach_plugins/path/cartesian_path_generation.h" + +#include "moveit_reach_plugins/utils.h" + +#include + +#include +#include +#include +#include + +namespace { + +template +T clamp(const T& val, const T& low, const T& high) { + return std::max(low, std::min(val, high)); +} + +} // namespace + +namespace moveit_reach_plugins { +namespace path { + +CartesianPathGeneration::CartesianPathGeneration() : PathBase() {} + +bool CartesianPathGeneration::initialize( + const std::string& name, rclcpp::Node::SharedPtr node, + const std::shared_ptr model) { + // interface to outer ros params and descriptions + node_ = node; + model_ = model; + name_ = name; + + auto LOGGER = rclcpp::get_logger( + name_ + ".moveit_reach_plugins.CartesianPathGeneration"); + + std::string param_prefix = "path_generation_config." + name_ + "."; + // get parameters for cartesian path computation + try { + double global_tx = 0.0, global_ty = 0.0, global_tz = 0.0; + double global_rx = 0.0, global_ry = 0.0, global_rz = 0.0; + if (!node->get_parameter(param_prefix + "global_transformation.tx", + global_tx)) { + RCLCPP_ERROR(LOGGER, "No parameter defined by the name: '%s'", + (param_prefix + "global_transformation.tx").c_str()); + return false; + } + if (!node->get_parameter(param_prefix + "global_transformation.ty", + global_ty)) { + RCLCPP_ERROR(LOGGER, "No parameter defined by the name: '%s'", + (param_prefix + "global_transformation.ty").c_str()); + return false; + } + if (!node->get_parameter(param_prefix + "global_transformation.tz", + global_tz)) { + RCLCPP_ERROR(LOGGER, "No parameter defined by the name: '%s'", + (param_prefix + "global_transformation.tz").c_str()); + return false; + } + if (!node->get_parameter(param_prefix + "global_transformation.rx", + global_rx)) { + RCLCPP_ERROR(LOGGER, "No parameter defined by the name: '%s'", + (param_prefix + "global_transformation.rx").c_str()); + return false; + } + if (!node->get_parameter(param_prefix + "global_transformation.ry", + global_ry)) { + RCLCPP_ERROR(LOGGER, "No parameter defined by the name: '%s'", + (param_prefix + "global_transformation.ry").c_str()); + return false; + } + if (!node->get_parameter(param_prefix + "global_transformation.rz", + global_rz)) { + RCLCPP_ERROR(LOGGER, "No parameter defined by the name: '%s'", + (param_prefix + "global_transformation.rz").c_str()); + return false; + } + // init global transformation + global_transformation_ = + Eigen::Translation3d(global_tx, global_ty, global_tz); + global_transformation_ = + global_transformation_ * + Eigen::AngleAxisd(global_rx * M_PI / 180.0, Eigen::Vector3d::UnitX()); + global_transformation_ = + global_transformation_ * + Eigen::AngleAxisd(global_ry * M_PI / 180.0, Eigen::Vector3d::UnitY()); + global_transformation_ = + global_transformation_ * + Eigen::AngleAxisd(global_rz * M_PI / 180.0, Eigen::Vector3d::UnitZ()); + + double tool_tx = 0.0, tool_ty = 0.0, tool_tz = 0.0; + double tool_rx = 0.0, tool_ry = 0.0, tool_rz = 0.0; + if (!node->get_parameter(param_prefix + "tool_transformation.tx", + tool_tx)) { + RCLCPP_ERROR(LOGGER, "No parameter defined by the name: '%s'", + (param_prefix + "tool_transformation.tx").c_str()); + return false; + } + if (!node->get_parameter(param_prefix + "tool_transformation.ty", + tool_ty)) { + RCLCPP_ERROR(LOGGER, "No parameter defined by the name: '%s'", + (param_prefix + "tool_transformation.ty").c_str()); + return false; + } + if (!node->get_parameter(param_prefix + "tool_transformation.tz", + tool_tz)) { + RCLCPP_ERROR(LOGGER, "No parameter defined by the name: '%s'", + (param_prefix + "tool_transformation.tz").c_str()); + return false; + } + if (!node->get_parameter(param_prefix + "tool_transformation.rx", + tool_rx)) { + RCLCPP_ERROR(LOGGER, "No parameter defined by the name: '%s'", + (param_prefix + "tool_transformation.rx").c_str()); + return false; + } + if (!node->get_parameter(param_prefix + "tool_transformation.ry", + tool_ry)) { + RCLCPP_ERROR(LOGGER, "No parameter defined by the name: '%s'", + (param_prefix + "tool_transformation.ry").c_str()); + return false; + } + if (!node->get_parameter(param_prefix + "tool_transformation.rz", + tool_rz)) { + RCLCPP_ERROR(LOGGER, "No parameter defined by the name: '%s'", + (param_prefix + "tool_transformation.rz").c_str()); + return false; + } + + // init tool transformation + tool_transformation_ = Eigen::Translation3d(tool_tx, tool_ty, tool_tz); + tool_transformation_ = + tool_transformation_ * + Eigen::AngleAxisd(tool_rx * M_PI / 180.0, Eigen::Vector3d::UnitX()); + tool_transformation_ = + tool_transformation_ * + Eigen::AngleAxisd(tool_ry * M_PI / 180.0, Eigen::Vector3d::UnitY()); + tool_transformation_ = + tool_transformation_ * + Eigen::AngleAxisd(tool_rz * M_PI / 180.0, Eigen::Vector3d::UnitZ()); + + if (!node->get_parameter(param_prefix + "distance_threshold", + distance_threshold_)) { + RCLCPP_ERROR(LOGGER, "No parameter defined by the name: '%s'", + (param_prefix + "distance_threshold").c_str()); + return false; + } + + if (!node->get_parameter(param_prefix + "max_eef_step", max_eef_step_)) { + RCLCPP_ERROR(LOGGER, "No parameter defined by the name: '%s'", + (param_prefix + "max_eef_step").c_str()); + return false; + } + if (!node->get_parameter(param_prefix + "jump_threshold", + jump_threshold_)) { + RCLCPP_ERROR(LOGGER, "No parameter defined by the name: '%s'", + (param_prefix + "jump_threshold").c_str()); + return false; + } + if (!node->get_parameter(param_prefix + "tool_frame", tool_frame_)) { + RCLCPP_ERROR(LOGGER, "No parameter defined by the name: '%s'", + (param_prefix + "tool_frame").c_str()); + return false; + } + if (!node->get_parameter(param_prefix + "max_velocity_scaling_factor", + max_velocity_scaling_factor_)) { + RCLCPP_ERROR(LOGGER, "No parameter defined by the name: '%s'", + (param_prefix + "max_velocity_scaling_factor").c_str()); + return false; + } + + if (!node->get_parameter(param_prefix + "max_acceleration_scaling_factor", + max_acceleration_scaling_factor_)) { + RCLCPP_ERROR(LOGGER, "No parameter defined by the name: '%s'", + (param_prefix + "max_acceleration_scaling_factor").c_str()); + return false; + } + + if (!node->get_parameter("path_generation_config.collision_mesh_package", + collision_mesh_package_)) { + RCLCPP_ERROR(LOGGER, "No parameter defined by the name: '%s'", + "path_generation_config.collision_mesh_package"); + return false; + } + + if (!node->get_parameter("path_generation_config.collision_mesh_frame", + collision_mesh_frame_)) { + RCLCPP_ERROR(LOGGER, "No parameter defined by the name: '%s'", + "path_generation_config.collision_mesh_frame"); + return false; + } + + // make sure it is positive to follow solvers logic + retrieval_path_length_ = std::abs(double(retrieval_path_length_)); + max_eef_step_ = std::abs(double(max_eef_step_)); + jump_threshold_ = std::abs(double(jump_threshold_)); + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM(LOGGER, ex.what()); + return false; + } + + // init planning scene + scene_ = std::make_shared(model_); + + // Check that the input collision mesh frame exists + if (!scene_->knowsFrameTransform(collision_mesh_frame_)) { + RCLCPP_ERROR_STREAM(LOGGER, "Specified collision mesh frame '" + << collision_mesh_frame_ + << "' does not exist"); + return false; + } + + // Add the collision object to the planning scene + const std::string object_name = "reach_object"; + moveit_msgs::msg::CollisionObject obj = utils::createCollisionObject( + collision_mesh_package_, collision_mesh_frame_, object_name); + if (!scene_->processCollisionObjectMsg(obj)) { + RCLCPP_ERROR(LOGGER, "Failed to add collision mesh to planning scene"); + return false; + } + + std::string planning_group; + if (!node->get_parameter("planning_group", planning_group)) { + return false; + } + + jmg_ = model_->getJointModelGroup(planning_group); + if (!jmg_) { + RCLCPP_ERROR_STREAM(LOGGER, "Failed to get joint model group for '" + << planning_group << "'"); + return false; + } + + // output message about successful initialization + RCLCPP_INFO(LOGGER, "Successfully initialized '%s' plugin", + (name_ + ".CartesianPathGeneration").c_str()); + return true; +} + +std::optional CartesianPathGeneration::solvePath( + const std::map& start_state, + std::map& end_state, double& fraction, + moveit_msgs::msg::RobotTrajectory& moveit_trajectory) { + moveit::core::RobotState state(model_); + + const std::vector& joint_names = + jmg_->getActiveJointModelNames(); + + std::vector start_state_subset; + if (!utils::transcribeInputMap(start_state, joint_names, + start_state_subset)) { + auto LOGGER = rclcpp::get_logger( + name_ + ".moveit_reach_plugins.CartesianPathGeneration"); + RCLCPP_ERROR_STREAM( + LOGGER, __FUNCTION__ << ": failed to transcribe input pose map"); + return {}; + } + + state.setJointGroupPositions(jmg_, start_state_subset); + state.update(); + + const Eigen::Isometry3d& target = state.getGlobalLinkTransform(tool_frame_); + + std::vector> trajectory; + + // transform in global and local frame + Eigen::Isometry3d final_target = + global_transformation_ * target * tool_transformation_; + + double f = moveit::core::CartesianInterpolator::computeCartesianPath( + &state, jmg_, trajectory, state.getLinkModel(tool_frame_), final_target, + true, moveit::core::MaxEEFStep(max_eef_step_), + moveit::core::JumpThreshold(jump_threshold_, jump_threshold_), + std::bind(&CartesianPathGeneration::isIKSolutionValid, this, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); + + if (f != 0.0) { + // moveit trajectory + auto result = + std::make_shared(model_, jmg_); + for (const auto& waypoint : trajectory) + result->addSuffixWayPoint(waypoint, 0.0); + trajectory_processing::IterativeParabolicTimeParameterization timing; + timing.computeTimeStamps(*result, max_velocity_scaling_factor_, + max_acceleration_scaling_factor_); + result->getRobotTrajectoryMsg(moveit_trajectory); + + std::vector end_state_vec; + result->getLastWayPoint().copyJointGroupPositions(jmg_, end_state_vec); + end_state = [&] { + std::map ret; + for (size_t i = 0; i < joint_names.size(); ++i) { + ret[joint_names[i]] = end_state_vec[i]; + } + return ret; + }(); + + // set fraction + fraction = f; + return f; + + } else { + fraction = 0.0; + return {}; + } +} + +std::vector CartesianPathGeneration::getJointNames() const { + return jmg_->getJointModelNames(); +} + +bool CartesianPathGeneration::isIKSolutionValid( + moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg, + const double* ik_solution) const { + state->setJointGroupPositions(jmg, ik_solution); + state->update(); + + const bool colliding = + scene_->isStateColliding(*state, jmg->getName(), false); + const bool too_close = + (scene_->distanceToCollision( + *state, scene_->getAllowedCollisionMatrix()) < distance_threshold_); + return (!colliding && !too_close); +} + +} // namespace path +} // namespace moveit_reach_plugins + +#include +PLUGINLIB_EXPORT_CLASS(moveit_reach_plugins::path::CartesianPathGeneration, + reach::plugins::PathBase) diff --git a/moveit_reach_plugins/src/utils.cpp b/moveit_reach_plugins/src/utils.cpp index c4038699..edd8a349 100644 --- a/moveit_reach_plugins/src/utils.cpp +++ b/moveit_reach_plugins/src/utils.cpp @@ -105,9 +105,9 @@ visualization_msgs::msg::Marker makeVisual( marker.color.a = 1.0; // Don't forget to set the alpha! if (r.reached) { - marker.color.r = 1.0 - r.retrieved_fraction; - marker.color.g = 1.0 - r.retrieved_fraction; - marker.color.b = r.retrieved_fraction; + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; } else { marker.color.r = 1.0; diff --git a/reach_core/CMakeLists.txt b/reach_core/CMakeLists.txt index fa2a993b..24d7149d 100644 --- a/reach_core/CMakeLists.txt +++ b/reach_core/CMakeLists.txt @@ -21,6 +21,7 @@ find_package(visualization_msgs REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) find_package(std_msgs REQUIRED) find_package(moveit_msgs REQUIRED) +find_package(yaml-cpp) find_package(OpenMP) @@ -109,13 +110,15 @@ add_library(${PROJECT_NAME} src/core/reach_visualizer.cpp # Reach Study src/core/reach_study.cpp + # path generation + src/core/path_generation.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC $ $ ) -target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_utils) +target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_utils yaml-cpp) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS} ) diff --git a/reach_core/include/reach_core/path_generation.h b/reach_core/include/reach_core/path_generation.h new file mode 100644 index 00000000..6783b5f3 --- /dev/null +++ b/reach_core/include/reach_core/path_generation.h @@ -0,0 +1,113 @@ +/* + * Copyright 2022 PickNik, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +/* Authors: Lovro Ivanov, @livanov93 + Desc: +*/ +#ifndef REACH_CORE_PATH_GENERATION_H +#define REACH_CORE_PATH_GENERATION_H + +#include "geometry_msgs/msg/pose_array.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "reach_core/plugins/path_base.h" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +constexpr char PACKAGE[] = "reach_core"; +constexpr char PATH_BASE_CLASS[] = "reach::plugins::PathBase"; +constexpr char IK_BASE_CLASS[] = "reach::plugins::IKSolverBase"; +constexpr char DISPLAY_BASE_CLASS[] = "reach::plugins::DisplayBase"; + +namespace reach { +namespace core { + +/// \brief Holds parameters for the \p PathGeneration +class Configuration { + public: + Configuration() + : path_loader(PACKAGE, PATH_BASE_CLASS), + solver_loader(PACKAGE, IK_BASE_CLASS), + display_loader(PACKAGE, DISPLAY_BASE_CLASS) {} + + std::string results_package; + std::string results_directory; + std::string results_db_name; + + // external interfaces + ReachDatabasePtr db; + ReachVisualizerPtr visualizer; + + // plugin loaders + pluginlib::ClassLoader path_loader; + pluginlib::ClassLoader solver_loader; + pluginlib::ClassLoader display_loader; + + // plugins + std::vector path_generators; + reach::plugins::IKSolverBasePtr ik_solver; + reach::plugins::DisplayBasePtr display; + // robot model + std::shared_ptr robot_model; + /// \brief Utility for making a common \p PathGeneration configuration + /// \param node Used to initialize solvers + /// \param prefix + /// \returns Typical configuration + Configuration Make(std::shared_ptr node, + const std::string& prefix); +}; + +/** + * @brief The ReachStudy class + */ +class PathGeneration : public rclcpp::Node { + public: + /** + * @brief PathGeneration + * @param name + */ + PathGeneration(const std::string& name); + + ~PathGeneration(); + + private: + bool initialize(); + + bool visualizeDatabases(); + + bool getObjectPointCloud(); + + protected: + std::string getFullPathToDb(const std::string& db_pkg, + const std::string& db_dir, + const std::string& db_config, + const std::string& db_name); + + Configuration configuration_; +}; + +} // namespace core +} // namespace reach + +#endif // REACH_CORE_REACH_STUDY_H diff --git a/reach_core/include/reach_core/plugins/ik_solver_base.h b/reach_core/include/reach_core/plugins/ik_solver_base.h index 6134ce3a..08bfb053 100644 --- a/reach_core/include/reach_core/plugins/ik_solver_base.h +++ b/reach_core/include/reach_core/plugins/ik_solver_base.h @@ -61,10 +61,8 @@ class IKSolverBase { */ virtual std::optional solveIKFromSeed( const Eigen::Isometry3d& target, - const std::map& seed, std::vector& solution, - std::vector& joint_space_trajectory, - std::vector& cartesian_space_waypoints, double& fraction, - moveit_msgs::msg::RobotTrajectory& moveit_trajectory) = 0; + const std::map& seed, + std::vector& solution) = 0; /** * @brief getJointNames diff --git a/reach_core/include/reach_core/plugins/path_base.h b/reach_core/include/reach_core/plugins/path_base.h new file mode 100644 index 00000000..84d7e6a9 --- /dev/null +++ b/reach_core/include/reach_core/plugins/path_base.h @@ -0,0 +1,107 @@ +/* + * Copyright 2022 PickNik, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +/* Authors: Lovro Ivanov, @livanov93 + Desc: +*/ +#ifndef REACH_CORE_PLUGINS_PATH_GENERATOR_BASE_H +#define REACH_CORE_PLUGINS_PATH_GENERATOR_BASE_H + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace reach { +namespace plugins { + +/** + * @brief Base class generating PATH at a given reach study location + */ +class PathBase { + public: + PathBase() {} + + virtual ~PathBase() {} + + /** + * @brief initialize + * @param config + * @return + */ + virtual bool initialize( + const std::string& name, rclcpp::Node::SharedPtr node, + const std::shared_ptr model) = 0; + + /** + * @brief solveIKFromSeed attempts to find a valid IK solution for the given + * target pose starting from the input seed state. If a solution is found, the + * resulting IK solution is saved, and the pose is scored according to the + * specified cost function plugin + * @param target pose defining the goal + * @param start_state robot state in joint space at the start of trajectory + * @param end_state robot state in joint space at the end of trajectory + * @param fraction total generated fraction of the desired trajectory + * [0.0, 1.0] + * @param moveit_trajectory generated trajectory + * @return a boost optional type indicating the success of the PATH generation + + virtual std::optional solvePath( + const Eigen::Isometry3d& target, + const std::map& start_state, + std::map& end_state, double& fraction, + moveit_msgs::msg::RobotTrajectory& moveit_trajectory) = 0; + */ + + /** + * @brief solveIKFromSeed attempts to find a valid IK solution for the given + * target pose starting from the input seed state. If a solution is found, the + * resulting IK solution is saved, and the pose is scored according to the + * specified cost function plugin + * @param start_state robot state in joint space at the start of trajectory + * @param goal_state target defined as robot state in joint space - end of + * trajectory + * @param fraction total generated fraction of the desired trajectory + * [0.0, 1.0] + * @param moveit_trajectory generated trajectory + * @return a boost optional type indicating the success of the PATH generation + */ + virtual std::optional solvePath( + const std::map& start_state, + std::map& end_state, double& fraction, + moveit_msgs::msg::RobotTrajectory& moveit_trajectory) = 0; + + /** + * @brief getJointNames + * @return + */ + virtual std::vector getJointNames() const = 0; + + public: + rclcpp::Node::SharedPtr node_; +}; +typedef std::shared_ptr PathBasePtr; + +} // namespace plugins +} // namespace reach + +#endif // REACH_CORE_PLUGINS_PATH_GENERATOR_BASE_H diff --git a/reach_core/include/reach_core/reach_database.h b/reach_core/include/reach_core/reach_database.h index 2497cc6f..0d5dd3a0 100644 --- a/reach_core/include/reach_core/reach_database.h +++ b/reach_core/include/reach_core/reach_database.h @@ -42,10 +42,7 @@ reach_msgs::msg::ReachRecord makeRecord( const std::string& id, const bool reached, const geometry_msgs::msg::Pose& goal, const sensor_msgs::msg::JointState& seed_state, - const sensor_msgs::msg::JointState& goal_state, const double score, - const std::string& ik_solver_name, const std::vector& waypoints, - const std::vector& trajectory, double retrieved_fraction, - const moveit_msgs::msg::RobotTrajectory& moveit_trajectory); + const sensor_msgs::msg::JointState& goal_state, const double score); /** * @brief toMap @@ -55,6 +52,9 @@ reach_msgs::msg::ReachRecord makeRecord( std::map jointStateMsgToMap( const sensor_msgs::msg::JointState& state); +sensor_msgs::msg::JointState mapToJointStateMsg( + const std::map& state); + /** * @brief to vector of Map * @param trectory diff --git a/reach_core/include/reach_core/reach_study.h b/reach_core/include/reach_core/reach_study.h index 9b990565..837e186a 100644 --- a/reach_core/include/reach_core/reach_study.h +++ b/reach_core/include/reach_core/reach_study.h @@ -26,25 +26,41 @@ #include #include #include +#include #include #include #include #include -// -// namespace moveit -//{ -// namespace core -// { -// class RobotModel; -// typedef std::shared_ptr RobotModelConstPtr; -// class JointModelGroup; -// } -//} +bool transcribeInputMap(const std::map& input, + const std::vector& joint_names, + std::vector& input_subset) { + if (joint_names.size() > input.size()) { + return false; + } + + // Pull the joints of the planning group out of the input map + std::vector tmp; + tmp.reserve(joint_names.size()); + for (const std::string& name : joint_names) { + const auto it = input.find(name); + if (it == input.end()) { + return false; + } else { + tmp.push_back(it->second); + } + } + + input_subset = std::move(tmp); + + return true; +} namespace reach { namespace core { +typedef std::map> + RobotConfigurations; /** * @brief The ReachStudy class */ @@ -75,6 +91,10 @@ class ReachStudy { private: bool initializeStudy(const StudyParameters& sp); + bool initializePathGeneration(const StudyParameters& sp); + + void generatePaths(); + bool getReachObjectPointCloud(); void runInitialReachStudy(); @@ -96,8 +116,13 @@ class ReachStudy { // Plugins pluginlib::ClassLoader solver_loader_; pluginlib::ClassLoader display_loader_; + pluginlib::ClassLoader path_generator_loader_; reach::plugins::IKSolverBasePtr ik_solver_; reach::plugins::DisplayBasePtr display_; + std::vector path_generators_; + ReachDatabasePtr paths_db_; + RobotConfigurations robot_configurations_; + std::string paths_path_; ReachVisualizerPtr visualizer_; @@ -110,7 +135,6 @@ class ReachStudy { sensor_msgs::msg::PointCloud2 cloud_msg_; std::shared_ptr node_; - rclcpp::Publisher::SharedPtr ps_pub_; rclcpp::Publisher::SharedPtr done_pub_; // robot model diff --git a/reach_core/include/reach_core/study_parameters.h b/reach_core/include/reach_core/study_parameters.h index 49977d75..f4465f22 100644 --- a/reach_core/include/reach_core/study_parameters.h +++ b/reach_core/include/reach_core/study_parameters.h @@ -45,8 +45,8 @@ struct StudyOptimization { * reach study */ struct StudyParameters { - // XmlRpc::XmlRpcValue ik_solver_config; - // XmlRpc::XmlRpcValue display_config; + std::string tool_frame; + std::string ik_solver_config_name; std::string display_config_name; StudyOptimization optimization; @@ -66,6 +66,22 @@ struct StudyParameters { std::vector initial_seed_state; bool keep_running; bool invert_z_tool_rotation; + + // path generator stuff + bool generate_paths; + bool initial_source_db; + bool initial_source_robot_configurations; + std::vector paths; + std::vector path_based_plugins; + // initial source db + std::string db_name; + std::string db_package; + std::string db_dir; + std::string db_config_name; + // initial source robot configs + std::string robot_configurations_name; + std::string robot_configurations_package; + std::string robot_configurations_dir; }; } // namespace core diff --git a/reach_core/include/reach_core/utils/visualization_utils.h b/reach_core/include/reach_core/utils/visualization_utils.h index aec0c9fb..1b68a0be 100644 --- a/reach_core/include/reach_core/utils/visualization_utils.h +++ b/reach_core/include/reach_core/utils/visualization_utils.h @@ -37,19 +37,6 @@ visualization_msgs::msg::Marker makeVisual( const std::string& ns = "reach", const boost::optional>& color = {}); -/** - * @brief makeInteractiveMarker - * @param r - * @param frame - * @param scale - * @return - */ -visualization_msgs::msg::Marker makeVisualTraj( - const rclcpp::Node::SharedPtr& node, const reach_msgs::msg::ReachRecord& r, - const std::string& frame, const double scale, - const std::string& ns = "reach", - const boost::optional>& color = {}); - /** * @brief makeInteractiveMarker * @param r diff --git a/reach_core/launch/start.launch.py b/reach_core/launch/start.launch.py index 802e18ad..f82358e4 100644 --- a/reach_core/launch/start.launch.py +++ b/reach_core/launch/start.launch.py @@ -215,10 +215,10 @@ def generate_launch_description(): ) nodes_to_run = [robot_reach_study_node, - control_node, + # control_node, robot_state_publisher_node, - joint_state_broadcaster_spawner, + # joint_state_broadcaster_spawner, run_move_group_node, rviz_node] - return LaunchDescription(declared_arguments + nodes_to_run) \ No newline at end of file + return LaunchDescription(declared_arguments + nodes_to_run) diff --git a/reach_core/package.xml b/reach_core/package.xml index 08919e4f..0a6d620f 100644 --- a/reach_core/package.xml +++ b/reach_core/package.xml @@ -29,6 +29,7 @@ moveit_ros_planning_interface std_msgs moveit_msgs + libyaml-cpp-dev diff --git a/reach_core/src/core/ik_helper.cpp b/reach_core/src/core/ik_helper.cpp index c33c21f3..6efd654f 100644 --- a/reach_core/src/core/ik_helper.cpp +++ b/reach_core/src/core/ik_helper.cpp @@ -109,13 +109,8 @@ NeighborReachResult reachNeighborsDirect( // Use current point's IK solution as seed std::vector new_solution; - std::vector new_cartesian_space_waypoints; - std::vector new_joint_space_trajectory; - double new_fraction = 0.0; - moveit_msgs::msg::RobotTrajectory new_moveit_trajectory; - std::optional score = solver->solveIKFromSeed( - target, previous_solution, new_solution, new_joint_space_trajectory, - new_cartesian_space_waypoints, new_fraction, new_moveit_trajectory); + std::optional score = + solver->solveIKFromSeed(target, previous_solution, new_solution); if (score) { // Calculate the joint distance between the seed and new goal states @@ -134,10 +129,6 @@ NeighborReachResult reachNeighborsDirect( msg.seed_state.position = rec.goal_state.position; msg.goal_state.position = new_solution; msg.score = *score; - msg.joint_space_trajectory = new_joint_space_trajectory; - msg.waypoints = new_cartesian_space_waypoints; - msg.retrieved_fraction = new_fraction; - msg.moveit_trajectory = new_moveit_trajectory; db->put(msg); } @@ -184,14 +175,9 @@ void reachNeighborsRecursive(ReachDatabasePtr db, Eigen::Isometry3d target; tf2::fromMsg(neighbors[i].goal, target); - std::vector cartesian_space_waypoints; - std::vector joint_space_trajectory; - double fraction; - moveit_msgs::msg::RobotTrajectory moveit_trajectory; // Use current point's IK solution as seed - std::optional score = solver->solveIKFromSeed( - target, current_pose_map, new_pose, joint_space_trajectory, - cartesian_space_waypoints, fraction, moveit_trajectory); + std::optional score = + solver->solveIKFromSeed(target, current_pose_map, new_pose); if (score) { // Calculate the joint distance between the seed and new goal states for (std::size_t j = 0; j < current_pose.size(); ++j) { @@ -204,7 +190,6 @@ void reachNeighborsRecursive(ReachDatabasePtr db, new_rec.goal_state.position = new_pose; new_rec.reached = true; new_rec.score = *score; - new_rec.moveit_trajectory = moveit_trajectory; // Recursively enter this function at the new neighboring location reachNeighborsRecursive(db, new_rec, solver, radius, result, diff --git a/reach_core/src/core/path_generation.cpp b/reach_core/src/core/path_generation.cpp new file mode 100644 index 00000000..a4ad1c92 --- /dev/null +++ b/reach_core/src/core/path_generation.cpp @@ -0,0 +1,96 @@ +/* + * Copyright 2022 PickNik, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +/* Authors: Lovro Ivanov, @livanov93 + Desc: +*/ + +#include "reach_core/path_generation.h" + +#include + +#include // for RobotModelLoader + +namespace reach { +namespace core { + +Configuration Configuration::Make(std::shared_ptr node, + const std::string& prefix) { + try { + auto const loader = + robot_model_loader::RobotModelLoader(node, "robot_description"); + auto const model = loader.getModel(); + if (model == nullptr) { + throw std::runtime_error( + "'robot_description' parameter parsing resulted in nullptr"); + } + + // fetch parameters + node->get_parameter("ik_solver_config"); + + Configuration c; + c.results_package = ""; + c.results_directory = ""; + c.results_db_name = ""; + c.db = std::make_shared(); + c.robot_model = model; + c.ik_solver = c.solver_loader.createSharedInstance(""); + c.display = c.display_loader.createSharedInstance(""); + c.path_generators.push_back(c.path_loader.createSharedInstance("")); + + return c; + + } catch (std::exception const& e) { + throw std::runtime_error( + "Could not generate configuration for PathGeneration"); + } +} + +PathGeneration::PathGeneration(const std::string& name) : rclcpp::Node(name) {} + +PathGeneration::~PathGeneration() {} + +bool PathGeneration::initialize() { return false; } + +bool PathGeneration::visualizeDatabases() { return false; } + +bool PathGeneration::getObjectPointCloud() { return false; } + +std::string PathGeneration::getFullPathToDb(const std::string& db_pkg, + const std::string& db_dir, + const std::string& db_config, + const std::string& db_name) { + // Create a directory to store results of study + + std::string full_path_to_db; + std::string path_tmp = + ament_index_cpp::get_package_share_directory(db_pkg) + "/" + db_dir; + if (!path_tmp.empty() && std::filesystem::exists(path_tmp.c_str())) { + full_path_to_db = path_tmp + "/"; + } else { + throw std::runtime_error("Could not obtain full path to database"); + } + + full_path_to_db = full_path_to_db + db_config + "/" + db_name; + + if (!std::filesystem::exists(full_path_to_db.c_str())) { + throw std::runtime_error("Database does not exist"); + } + + return full_path_to_db; +} + +} // namespace core +} // namespace reach \ No newline at end of file diff --git a/reach_core/src/core/reach_database.cpp b/reach_core/src/core/reach_database.cpp index 3296deb6..28855de2 100644 --- a/reach_core/src/core/reach_database.cpp +++ b/reach_core/src/core/reach_database.cpp @@ -49,10 +49,7 @@ reach_msgs::msg::ReachRecord makeRecord( const std::string& id, const bool reached, const geometry_msgs::msg::Pose& goal, const sensor_msgs::msg::JointState& seed_state, - const sensor_msgs::msg::JointState& goal_state, const double score, - const std::string& ik_solver_name, const std::vector& waypoints, - const std::vector& trajectory, double retrieved_fraction, - const moveit_msgs::msg::RobotTrajectory& moveit_trajectory) { + const sensor_msgs::msg::JointState& goal_state, const double score) { reach_msgs::msg::ReachRecord r; r.id = id; r.goal = goal; @@ -60,11 +57,6 @@ reach_msgs::msg::ReachRecord makeRecord( r.seed_state = seed_state; r.goal_state = goal_state; r.score = score; - r.retrieved_fraction = retrieved_fraction; - r.ik_solver = ik_solver_name; - r.joint_space_trajectory = trajectory; - r.waypoints = waypoints; - r.moveit_trajectory = moveit_trajectory; return r; } @@ -77,6 +69,16 @@ std::map jointStateMsgToMap( return out; } +sensor_msgs::msg::JointState mapToJointStateMsg( + const std::map& state) { + sensor_msgs::msg::JointState out; + for (auto it = state.begin(); it != state.end(); ++it) { + out.name.push_back(it->first); + out.position.push_back(it->second); + } + return out; +} + std::vector> jointStateArrayToArrayOfMaps( const std::vector& trajectory, const std::vector& names) { diff --git a/reach_core/src/core/reach_study.cpp b/reach_core/src/core/reach_study.cpp index 6a4283a6..14f69254 100644 --- a/reach_core/src/core/reach_study.cpp +++ b/reach_core/src/core/reach_study.cpp @@ -30,6 +30,9 @@ #include #include +// yaml stuff +#include + constexpr char SAMPLE_MESH_SRV_TOPIC[] = "sample_mesh"; const static double SRV_TIMEOUT = 5.0; constexpr char INPUT_CLOUD_TOPIC[] = "input_cloud"; @@ -38,19 +41,21 @@ constexpr char OPT_SAVED_DB_NAME[] = "optimized_reach.db"; namespace reach { namespace core { -namespace { -const rclcpp::Logger LOGGER = rclcpp::get_logger("reach_core.reach_visualizer"); -} + constexpr char PACKAGE[] = "reach_core"; constexpr char IK_BASE_CLASS[] = "reach::plugins::IKSolverBase"; constexpr char DISPLAY_BASE_CLASS[] = "reach::plugins::DisplayBase"; +constexpr char PATH_BASE_CLASS[] = "reach::plugins::PathBase"; ReachStudy::ReachStudy(const rclcpp::Node::SharedPtr node) : node_(node), cloud_(new pcl::PointCloud()), db_(new ReachDatabase()), + paths_db_(new ReachDatabase()), solver_loader_(PACKAGE, IK_BASE_CLASS), - display_loader_(PACKAGE, DISPLAY_BASE_CLASS) {} + display_loader_(PACKAGE, DISPLAY_BASE_CLASS), + path_generator_loader_(PACKAGE, PATH_BASE_CLASS) {} + ReachStudy::~ReachStudy() { ik_solver_.reset(); display_.reset(); @@ -59,7 +64,10 @@ ReachStudy::~ReachStudy() { cloud_.reset(); node_.reset(); model_.reset(); - ps_pub_.reset(); + paths_db_.reset(); + for (size_t i = 0; i < path_generators_.size(); ++i) { + path_generators_[i].reset(); + } } bool ReachStudy::initializeStudy(const StudyParameters& sp) { @@ -69,15 +77,14 @@ bool ReachStudy::initializeStudy(const StudyParameters& sp) { model_ = moveit::planning_interface::getSharedRobotModel(node_, "robot_description"); - ps_pub_ = node_->create_publisher( - "pose_stamped", 1); + // expose information if analysis has finished done_pub_ = node_->create_publisher("analysis_done", 1); try { ik_solver_ = solver_loader_.createSharedInstance(sp_.ik_solver_config_name); display_ = display_loader_.createSharedInstance(sp_.display_config_name); } catch (const pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(node_->get_logger(), "Pluginlib exception thrown while creating shared instances " "of ik solver and/or display: '%s'", ex.what()); @@ -85,7 +92,7 @@ bool ReachStudy::initializeStudy(const StudyParameters& sp) { display_.reset(); return false; } catch (const std::exception& ex) { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(node_->get_logger(), "Error while creating shared instances of ik solver and/or " "display: '%s'", ex.what()); @@ -97,7 +104,7 @@ bool ReachStudy::initializeStudy(const StudyParameters& sp) { // Initialize the IK solver plugin and display plugin if (!ik_solver_->initialize(sp_.ik_solver_config_name, node_, model_) || !display_->initialize(sp_.display_config_name, node_, model_)) { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(node_->get_logger(), "Could not initialized both display and ik solver plugins!"); ik_solver_.reset(); display_.reset(); @@ -113,8 +120,8 @@ bool ReachStudy::initializeStudy(const StudyParameters& sp) { } else { dir_ = ament_index_cpp::get_package_share_directory("reach_core") + "/results/"; - RCLCPP_WARN(LOGGER, "Using default results file directory: '%s'", - dir_.c_str()); + RCLCPP_WARN(node_->get_logger(), + "Using default results file directory: '%s'", dir_.c_str()); } results_dir_ = dir_ + sp_.config_name + "/"; const char* char_dir = results_dir_.c_str(); @@ -138,13 +145,14 @@ bool ReachStudy::run(const StudyParameters& sp) { // Initialize the study if (!initializeStudy(sp)) { - RCLCPP_ERROR(LOGGER, "Failed to initialize the reach study"); + RCLCPP_ERROR(node_->get_logger(), "Failed to initialize the reach study"); return false; } // Get the reach object point cloud if (!getReachObjectPointCloud()) { - RCLCPP_ERROR(LOGGER, "Unable to obtain reach object point cloud"); + RCLCPP_ERROR(node_->get_logger(), + "Unable to obtain reach object point cloud"); ik_solver_.reset(); display_.reset(); return false; @@ -164,22 +172,21 @@ bool ReachStudy::run(const StudyParameters& sp) { // Attempt to load previously saved optimized reach_study database if (!db_->load(results_dir_ + OPT_SAVED_DB_NAME)) { - RCLCPP_INFO(LOGGER, "Unable to load optimized database at '%s'!", + RCLCPP_INFO(node_->get_logger(), + "Unable to load optimized database at '%s'!", (results_dir_ + OPT_SAVED_DB_NAME).c_str()); // Attempt to load previously saved initial reach study database if (!db_->load(results_dir_ + SAVED_DB_NAME)) { - RCLCPP_INFO(LOGGER, "------------------------------"); - RCLCPP_INFO(LOGGER, "No reach study database loaded"); - RCLCPP_INFO(LOGGER, "------------------------------"); + RCLCPP_INFO(node_->get_logger(), "------------------------------"); + RCLCPP_INFO(node_->get_logger(), "No reach study database loaded"); + RCLCPP_INFO(node_->get_logger(), "------------------------------"); // Run the first pass of the reach study runInitialReachStudy(); db_->printResults(); visualizer_->update(); // check if we don't have to optimize - if (sp.run_initial_study_only || - (sp.ik_solver_config_name == - "moveit_reach_plugins/ik/PlannerBasedIKSolver")) { + if (sp.run_initial_study_only) { done_pub_->publish(std_msgs::msg::Empty()); while (rclcpp::ok() && sp_.keep_running) { // keep it running @@ -190,11 +197,11 @@ bool ReachStudy::run(const StudyParameters& sp) { return true; } } else { - RCLCPP_INFO(LOGGER, + RCLCPP_INFO(node_->get_logger(), "----------------------------------------------------"); - RCLCPP_INFO(LOGGER, + RCLCPP_INFO(node_->get_logger(), "Unoptimized reach study database successfully loaded"); - RCLCPP_INFO(LOGGER, + RCLCPP_INFO(node_->get_logger(), "----------------------------------------------------"); db_->printResults(); @@ -206,14 +213,14 @@ bool ReachStudy::run(const StudyParameters& sp) { // when new type of plugin is created motion generation will be option // that is doable on existing databases created by other ik solvers and // optimization procedures - ugly but... - if (sp.run_initial_study_only || - (sp.ik_solver_config_name == - "moveit_reach_plugins/ik/PlannerBasedIKSolver")) { + if (sp.run_initial_study_only) { done_pub_->publish(std_msgs::msg::Empty()); + while (rclcpp::ok() && sp_.keep_running) { // keep it running std::this_thread::sleep_for(std::chrono::milliseconds(100)); } + db_->calculateResults(); db_->save(results_dir_ + SAVED_DB_NAME); return true; @@ -238,16 +245,33 @@ bool ReachStudy::run(const StudyParameters& sp) { optimizeReachStudyResults(); db_->printResults(); visualizer_->update(); + + // TODO(livanov93): add path based plugin run on optimized database } else { - RCLCPP_INFO(LOGGER, "--------------------------------------------------"); - RCLCPP_INFO(LOGGER, "Optimized reach study database successfully loaded"); - RCLCPP_INFO(LOGGER, "--------------------------------------------------"); + RCLCPP_INFO(node_->get_logger(), + "--------------------------------------------------"); + RCLCPP_INFO(node_->get_logger(), + "Optimized reach study database successfully loaded"); + RCLCPP_INFO(node_->get_logger(), + "--------------------------------------------------"); + + db_->printResults(); + visualizer_->update(); // TODO(livanov93): add here run of planning based plugin - add new type of // plugin + if (sp_.generate_paths) { + if (!initializePathGeneration(sp_)) { + RCLCPP_ERROR(node_->get_logger(), + "Path Generation could not be initialized"); + return false; + } - db_->printResults(); - visualizer_->update(); + // path generation + generatePaths(); + + paths_db_->save(paths_path_); + } } // Find the average number of neighboring points can be reached by the robot @@ -264,21 +288,22 @@ bool ReachStudy::run(const StudyParameters& sp) { // Compare database results if (!sp_.compare_dbs.empty()) { if (!compareDatabases()) { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(node_->get_logger(), "Unable to compare the current reach study database with " "the other specified databases"); } } if (!sp_.visualize_dbs.empty()) { - RCLCPP_INFO(LOGGER, "Visualizing databases..."); + RCLCPP_INFO(node_->get_logger(), "Visualizing databases..."); if (!visualizeDatabases()) { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(node_->get_logger(), "Unable to compare the current reach study database with " "the other specified databases"); } } } + // analysis done information sharing done_pub_->publish(std_msgs::msg::Empty()); while (rclcpp::ok() && sp_.keep_running) { @@ -289,6 +314,9 @@ bool ReachStudy::run(const StudyParameters& sp) { ik_solver_.reset(); display_.reset(); visualizer_.reset(); + for (size_t i = 0; i < path_generators_.size(); ++i) { + path_generators_[i].reset(); + } return true; } @@ -309,7 +337,8 @@ bool ReachStudy::getReachObjectPointCloud() { req->fixed_frame = sp_.fixed_frame; req->object_frame = sp_.object_frame; - RCLCPP_INFO(LOGGER, "Waiting for service '%s'.", SAMPLE_MESH_SRV_TOPIC); + RCLCPP_INFO(node_->get_logger(), "Waiting for service '%s'.", + SAMPLE_MESH_SRV_TOPIC); client->wait_for_service(); bool success_tmp = false; bool inner_callback_finished = false; @@ -319,7 +348,8 @@ bool ReachStudy::getReachObjectPointCloud() { inner_future) { success_tmp = inner_future.get()->success; cloud_msg_ = inner_future.get()->cloud; - RCLCPP_DEBUG(LOGGER, "Inner service callback message: '%s'", + RCLCPP_DEBUG(node_->get_logger(), + "Inner service callback message: '%s'", inner_future.get()->message.c_str()); inner_callback_finished = true; }; @@ -341,8 +371,9 @@ bool ReachStudy::getReachObjectPointCloud() { return true; } else { - RCLCPP_ERROR_STREAM(LOGGER, "Failed to call point cloud loading service '" - << client->get_service_name() << "'"); + RCLCPP_ERROR_STREAM(node_->get_logger(), + "Failed to call point cloud loading service '" + << client->get_service_name() << "'"); return false; } } @@ -357,138 +388,59 @@ void ReachStudy::runInitialReachStudy() { current_counter = previous_pct = 0; const int cloud_size = static_cast(cloud_->points.size()); - if (sp_.ik_solver_config_name != - "moveit_reach_plugins/ik/PlannerBasedIKSolver") { #pragma omp parallel for - for (int i = 0; i < cloud_size; ++i) { - // Get pose from point cloud array - const pcl::PointNormal& pt = cloud_->points[i]; - Eigen::Isometry3d tgt_frame; - tgt_frame = - utils::createFrame(pt.getArray3fMap(), pt.getNormalVector3fMap()); - if (sp_.invert_z_tool_rotation) { - tgt_frame = tgt_frame * tool_z_rot * tool_z_rot_align; - } else { - tgt_frame = tgt_frame * tool_z_rot; - } + for (int i = 0; i < cloud_size; ++i) { + // Get pose from point cloud array + const pcl::PointNormal& pt = cloud_->points[i]; + Eigen::Isometry3d tgt_frame; + tgt_frame = + utils::createFrame(pt.getArray3fMap(), pt.getNormalVector3fMap()); + if (sp_.invert_z_tool_rotation) { + tgt_frame = tgt_frame * tool_z_rot * tool_z_rot_align; + } else { + tgt_frame = tgt_frame * tool_z_rot; + } - // Get the seed position - sensor_msgs::msg::JointState seed_state; - seed_state.name = ik_solver_->getJointNames(); - if (seed_state.name.size() != sp_.initial_seed_state.size()) { - seed_state.position = std::vector(seed_state.name.size(), 0.0); - } else { - seed_state.position = sp_.initial_seed_state; - } + // Get the seed position + sensor_msgs::msg::JointState seed_state; + seed_state.name = ik_solver_->getJointNames(); + if (seed_state.name.size() != sp_.initial_seed_state.size()) { + seed_state.position = std::vector(seed_state.name.size(), 0.0); + } else { + seed_state.position = sp_.initial_seed_state; + } - // Solve IK - std::vector solution; - std::vector cartesian_space_waypoints; - std::vector joint_space_trajectory; - moveit_msgs::msg::RobotTrajectory moveit_trajectory; - double fraction; - std::optional score = ik_solver_->solveIKFromSeed( - tgt_frame, jointStateMsgToMap(seed_state), solution, - joint_space_trajectory, cartesian_space_waypoints, fraction, - moveit_trajectory); - - // Create objects to save in the reach record - geometry_msgs::msg::Pose tgt_pose; - tgt_pose = tf2::toMsg(tgt_frame); - - sensor_msgs::msg::JointState goal_state(seed_state); - - if (score) { - geometry_msgs::msg::PoseStamped tgt_pose_stamped; - tgt_pose_stamped.pose = tgt_pose; - tgt_pose_stamped.header.frame_id = cloud_msg_.header.frame_id; - - // fill the goal state - goal_state.position = solution; - - auto msg = makeRecord(std::to_string(i), true, tgt_pose, seed_state, - goal_state, *score, sp_.ik_solver_config_name, - cartesian_space_waypoints, joint_space_trajectory, - fraction, moveit_trajectory); - db_->put(msg); - } else { - auto msg = makeRecord(std::to_string(i), false, tgt_pose, seed_state, - goal_state, 0.0, sp_.ik_solver_config_name, {}, - {}, fraction, moveit_trajectory); - db_->put(msg); - } + // Solve IK + std::vector solution; + std::optional score = ik_solver_->solveIKFromSeed( + tgt_frame, jointStateMsgToMap(seed_state), solution); - // Print function progress - current_counter++; - utils::integerProgressPrinter(current_counter, previous_pct, cloud_size); - } - } else { - // if planner based ik solver is used don't use omp - // TODO(livanov93): this is ugly - find other way, potentially create - // another type of plugin which will do motion generation on existing - // databases created by other ik solvers - for (int i = 0; i < cloud_size; ++i) { - // Get pose from point cloud array - const pcl::PointNormal& pt = cloud_->points[i]; - Eigen::Isometry3d tgt_frame; - tgt_frame = - utils::createFrame(pt.getArray3fMap(), pt.getNormalVector3fMap()); - if (sp_.invert_z_tool_rotation) { - tgt_frame = tgt_frame * tool_z_rot * tool_z_rot_align; - } else { - tgt_frame = tgt_frame * tool_z_rot; - } + // Create objects to save in the reach record + geometry_msgs::msg::Pose tgt_pose; + tgt_pose = tf2::toMsg(tgt_frame); - // Get the seed position - sensor_msgs::msg::JointState seed_state; - seed_state.name = ik_solver_->getJointNames(); - if (seed_state.name.size() != sp_.initial_seed_state.size()) { - seed_state.position = std::vector(seed_state.name.size(), 0.0); - } else { - seed_state.position = sp_.initial_seed_state; - } + sensor_msgs::msg::JointState goal_state(seed_state); - // Solve IK - std::vector solution; - std::vector cartesian_space_waypoints; - std::vector joint_space_trajectory; - double fraction; - moveit_msgs::msg::RobotTrajectory moveit_trajectory; - std::optional score = ik_solver_->solveIKFromSeed( - tgt_frame, jointStateMsgToMap(seed_state), solution, - joint_space_trajectory, cartesian_space_waypoints, fraction, - moveit_trajectory); - - // Create objects to save in the reach record - geometry_msgs::msg::Pose tgt_pose; - tgt_pose = tf2::toMsg(tgt_frame); - - sensor_msgs::msg::JointState goal_state(seed_state); - - if (score) { - geometry_msgs::msg::PoseStamped tgt_pose_stamped; - tgt_pose_stamped.pose = tgt_pose; - tgt_pose_stamped.header.frame_id = cloud_msg_.header.frame_id; - - // fill the goal state - goal_state.position = solution; - - auto msg = makeRecord(std::to_string(i), true, tgt_pose, seed_state, - goal_state, *score, sp_.ik_solver_config_name, - cartesian_space_waypoints, joint_space_trajectory, - fraction, moveit_trajectory); - db_->put(msg); - } else { - auto msg = makeRecord(std::to_string(i), false, tgt_pose, seed_state, - goal_state, 0.0, sp_.ik_solver_config_name, {}, - {}, fraction, moveit_trajectory); - db_->put(msg); - } + if (score) { + geometry_msgs::msg::PoseStamped tgt_pose_stamped; + tgt_pose_stamped.pose = tgt_pose; + tgt_pose_stamped.header.frame_id = cloud_msg_.header.frame_id; - // Print function progress - current_counter++; - utils::integerProgressPrinter(current_counter, previous_pct, cloud_size); + // fill the goal state + goal_state.position = solution; + + auto msg = makeRecord(std::to_string(i), true, tgt_pose, seed_state, + goal_state, *score); + db_->put(msg); + } else { + auto msg = makeRecord(std::to_string(i), false, tgt_pose, seed_state, + goal_state, 0.0); + db_->put(msg); } + + // Print function progress + current_counter++; + utils::integerProgressPrinter(current_counter, previous_pct, cloud_size); } // Save the results of the reach study to a database that we can query later @@ -497,8 +449,8 @@ void ReachStudy::runInitialReachStudy() { } void ReachStudy::optimizeReachStudyResults() { - RCLCPP_INFO(LOGGER, "----------------------"); - RCLCPP_INFO(LOGGER, "Beginning optimization"); + RCLCPP_INFO(node_->get_logger(), "----------------------"); + RCLCPP_INFO(node_->get_logger(), "Beginning optimization"); // Create sequential vector to be randomized std::vector rand_vec(db_->size()); @@ -513,7 +465,7 @@ void ReachStudy::optimizeReachStudyResults() { while (pct_improve > sp_.optimization.step_improvement_threshold && n_opt < sp_.optimization.max_steps) { - RCLCPP_INFO(LOGGER, "Entering optimization loop %d", n_opt); + RCLCPP_INFO(node_->get_logger(), "Entering optimization loop %d", n_opt); previous_score = db_->getStudyResults().norm_total_pose_score; current_counter = 0; previous_pct = 0; @@ -548,13 +500,15 @@ void ReachStudy::optimizeReachStudyResults() { db_->calculateResults(); db_->save(results_dir_ + OPT_SAVED_DB_NAME); - RCLCPP_INFO(LOGGER, "----------------------"); - RCLCPP_INFO(LOGGER, "Optimization concluded"); + RCLCPP_INFO(node_->get_logger(), "----------------------"); + RCLCPP_INFO(node_->get_logger(), "Optimization concluded"); } void ReachStudy::getAverageNeighborsCount() { - RCLCPP_INFO(LOGGER, "--------------------------------------------"); - RCLCPP_INFO(LOGGER, "Beginning average neighbor count calculation"); + RCLCPP_INFO(node_->get_logger(), + "--------------------------------------------"); + RCLCPP_INFO(node_->get_logger(), + "Beginning average neighbor count calculation"); std::atomic current_counter, previous_pct, neighbor_count; current_counter = previous_pct = neighbor_count = 0; @@ -586,9 +540,12 @@ void ReachStudy::getAverageNeighborsCount() { static_cast(neighbor_count.load()); RCLCPP_INFO_STREAM( - LOGGER, "Average number of neighbors reached: " << avg_neighbor_count); - RCLCPP_INFO_STREAM(LOGGER, "Average joint distance: " << avg_joint_distance); - RCLCPP_INFO(LOGGER, "------------------------------------------------"); + node_->get_logger(), + "Average number of neighbors reached: " << avg_neighbor_count); + RCLCPP_INFO_STREAM(node_->get_logger(), + "Average joint distance: " << avg_joint_distance); + RCLCPP_INFO(node_->get_logger(), + "------------------------------------------------"); db_->setAverageNeighborsCount(avg_neighbor_count); db_->setAverageJointDistance(avg_joint_distance); @@ -613,7 +570,7 @@ bool ReachStudy::compareDatabases() { for (size_t i = 0; i < db_filenames.size(); ++i) { ReachDatabase db; if (!db.load(db_filenames[i])) { - RCLCPP_ERROR(LOGGER, "Cannot load database at:\n %s", + RCLCPP_ERROR(node_->get_logger(), "Cannot load database at:\n %s", db_filenames[i].c_str()); continue; } @@ -622,7 +579,7 @@ bool ReachStudy::compareDatabases() { if (data.size() < 2) { RCLCPP_ERROR( - LOGGER, + node_->get_logger(), "Only %lu database(s) loaded; cannot compare fewer than 2 databases", data.size()); return false; @@ -651,7 +608,7 @@ bool ReachStudy::visualizeDatabases() { for (size_t i = 0; i < db_filenames.size(); ++i) { ReachDatabase db; if (!db.load(db_filenames[i])) { - RCLCPP_ERROR(LOGGER, "Cannot load database at:\n %s", + RCLCPP_ERROR(node_->get_logger(), "Cannot load database at:\n %s", db_filenames[i].c_str()); continue; } @@ -660,7 +617,7 @@ bool ReachStudy::visualizeDatabases() { if (data.size() < 2) { RCLCPP_ERROR( - LOGGER, + node_->get_logger(), "Only %lu database(s) loaded; cannot compare fewer than 2 databases", data.size()); return false; @@ -671,5 +628,134 @@ bool ReachStudy::visualizeDatabases() { return true; } +bool ReachStudy::initializePathGeneration(const StudyParameters& sp) { + // create instances + path_generators_.resize(sp.paths.size()); + for (size_t i = 0; i < sp.paths.size(); ++i) { + path_generators_[i] = + path_generator_loader_.createSharedInstance(sp.path_based_plugins[i]); + if (!path_generators_[i]->initialize(sp.paths[i], node_, model_)) { + RCLCPP_ERROR(node_->get_logger(), "Could not initialize '%s' '%s'", + sp.paths[i].c_str(), sp.path_based_plugins[i].c_str()); + return false; + } + } + + if (sp.initial_source_db) { + paths_path_ = ament_index_cpp::get_package_share_directory(sp.db_package) + + "/" + sp.db_dir + "/" + sp.db_config_name + "/" + sp.db_name; + if (!std::filesystem::exists(paths_path_)) { + return false; + } else { + bool loaded = paths_db_->load(paths_path_); + if (!loaded) return false; + } + } else if (sp.initial_source_robot_configurations) { + std::string yaml_path = ament_index_cpp::get_package_share_directory( + sp.robot_configurations_package) + + "/" + sp.robot_configurations_dir + "/" + + sp.robot_configurations_name; + // check if yaml exists + if (!std::filesystem::exists(yaml_path)) { + return false; + } + // load yaml + YAML::Node config = YAML::LoadFile(yaml_path); + for (const auto& robot_configuration : + config["robot_configurations"].as>()) { + robot_configurations_[robot_configuration] = + config[robot_configuration].as>(); + + // initialize target, seed state and goal state + reach_msgs::msg::ReachRecord r; + { + moveit::core::RobotState rs(model_); + auto jmg = model_->getJointModelGroup(sp_.planning_group); + if (!jmg) { + RCLCPP_ERROR_STREAM(node_->get_logger(), + "Failed to get joint model group for '" + << sp_.planning_group << "'"); + return false; + } + std::vector start_state_subset; + if (!transcribeInputMap(robot_configurations_[robot_configuration], + jmg->getJointModelNames(), + start_state_subset)) { + return false; + } + rs.setJointGroupPositions(jmg, start_state_subset); + r.id = robot_configuration; + r.goal = + tf2::toMsg(rs.getGlobalLinkTransform(sp_.fixed_frame).inverse() * + rs.getGlobalLinkTransform(sp_.tool_frame)); + r.goal_state = + mapToJointStateMsg(robot_configurations_[robot_configuration]); + r.seed_state = + mapToJointStateMsg(robot_configurations_[robot_configuration]); + } + paths_db_->put(r); + } + + // create database + std::string tmp_db_path = + ament_index_cpp::get_package_share_directory(sp.db_package) + "/" + + sp.db_dir + "/" + sp.db_config_name + "/"; + if (!std::filesystem::exists(tmp_db_path)) { + std::filesystem::path path(tmp_db_path.c_str()); + std::filesystem::create_directories(path); + } + paths_path_ = tmp_db_path + sp.db_name; + // save database + paths_db_->save(paths_path_); + + } else { + paths_db_ = nullptr; + return false; + } + + RCLCPP_INFO(node_->get_logger(), "Path Generation Initialized Successfully"); + return true; +} + +void ReachStudy::generatePaths() { + RCLCPP_INFO(node_->get_logger(), "Generating paths..."); + // for each record + for (auto it = paths_db_->begin(); it != paths_db_->end(); ++it) { + // get the whole record to update it with paths + reach_msgs::msg::ReachRecord r = it->second; + if (r.reached) { + // get target from db + auto start_state = jointStateMsgToMap(r.goal_state); + std::map end_state; + double fraction = 0.0; + moveit_msgs::msg::RobotTrajectory trajectory; + bool total_score_ok = false; + + // for each path generator + for (size_t i = 0; i < path_generators_.size(); ++i) { + std::optional score = path_generators_[i]->solvePath( + start_state, end_state, fraction, trajectory); + + // check if there is score + if (score) { + total_score_ok = true; + reach_msgs::msg::ReachPath path; + path.fraction = fraction; + path.moveit_trajectory = trajectory; + r.paths.push_back(path); + + // prepare for next path generator + start_state = end_state; + } else { + total_score_ok = false; + break; + } + } + paths_db_->put(r); + } + } + RCLCPP_INFO(node_->get_logger(), "Paths generated!"); +} + } // namespace core } // namespace reach diff --git a/reach_core/src/core/reach_visualizer.cpp b/reach_core/src/core/reach_visualizer.cpp index 718382b6..d8e3ecf8 100644 --- a/reach_core/src/core/reach_visualizer.cpp +++ b/reach_core/src/core/reach_visualizer.cpp @@ -87,13 +87,8 @@ void ReachVisualizer::reSolveIKCB( // Re-solve IK at the selected marker std::vector goal_pose; - std::vector cartesian_space_waypoints; - std::vector joint_space_trajectory; - double fraction; - moveit_msgs::msg::RobotTrajectory moveit_trajectory; - std::optional score = solver_->solveIKFromSeed( - target, seed_map, goal_pose, joint_space_trajectory, - cartesian_space_waypoints, fraction, moveit_trajectory); + std::optional score = + solver_->solveIKFromSeed(target, seed_map, goal_pose); // Update the database if the IK solution was valid if (score) { @@ -102,16 +97,13 @@ void ReachVisualizer::reSolveIKCB( lookup->reached = true; lookup->score = *score; lookup->goal_state.position = goal_pose; - lookup->joint_space_trajectory = joint_space_trajectory; - lookup->waypoints = cartesian_space_waypoints; - lookup->retrieved_fraction = fraction; - lookup->moveit_trajectory = moveit_trajectory; // Update the interactive marker server display_->updateInteractiveMarker(*lookup); display_->updateRobotPose(jointStateMsgToMap(lookup->goal_state)); - display_->updateRobotTrajectory(jointStateArrayToArrayOfMaps( - joint_space_trajectory, lookup->goal_state.name)); + + // display_->updateRobotTrajectory(jointStateArrayToArrayOfMaps( + // joint_space_trajectory, lookup->goal_state.name)); // Update the database db_->put(*lookup); @@ -131,8 +123,8 @@ void ReachVisualizer::showResultCB( auto lookup = db_->get(fb->marker_name); if (lookup) { display_->updateRobotPose(jointStateMsgToMap(lookup->goal_state)); - display_->updateRobotTrajectory(jointStateArrayToArrayOfMaps( - lookup->joint_space_trajectory, lookup->goal_state.name)); + // display_->updateRobotTrajectory(jointStateArrayToArrayOfMaps( + // lookup->joint_space_trajectory, lookup->goal_state.name)); } else { RCLCPP_ERROR_STREAM(LOGGER, "Record '" << fb->marker_name diff --git a/reach_core/src/robot_reach_study_node.cpp b/reach_core/src/robot_reach_study_node.cpp index ec6d23f4..758e5357 100644 --- a/reach_core/src/robot_reach_study_node.cpp +++ b/reach_core/src/robot_reach_study_node.cpp @@ -32,7 +32,8 @@ class RobotReachStudyNode : public rclcpp::Node { public: bool getStudyParameters(reach::core::StudyParameters& sp) { // fetch parameteres - if (!this->get_parameter("config_name", sp_.config_name) || + if (!this->get_parameter("tool_frame", sp_.tool_frame) || + !this->get_parameter("config_name", sp_.config_name) || !this->get_parameter("fixed_frame", sp_.fixed_frame) || !this->get_parameter("results_package", sp_.results_package) || !this->get_parameter("results_directory", sp_.results_directory) || @@ -104,6 +105,54 @@ class RobotReachStudyNode : public rclcpp::Node { this->get_parameter_or("invert_z_tool_rotation", sp_.invert_z_tool_rotation, true); + // to do path generation or not + this->get_parameter_or("generate_paths", sp_.generate_paths, false); + this->get_parameter_or("path_generation_config.path_plugins", sp_.paths, + {}); + for (const auto& p : sp_.paths) { + std::string tmp_plugin_name; + std::string tmp_param_name = + "path_generation_config." + p + ".plugin_name"; + this->get_parameter_or(tmp_param_name, tmp_plugin_name, + std::string("")); + sp_.path_based_plugins.push_back(tmp_plugin_name); + } + + this->get_parameter_or("path_generation_config.initial_source_db", + sp_.initial_source_db, false); + this->get_parameter_or( + "path_generation_config.initial_source_robot_configurations", + sp_.initial_source_robot_configurations, false); + + if (sp_.generate_paths && !sp_.initial_source_db && + !sp_.initial_source_robot_configurations) { + RCLCPP_ERROR(rclcpp::get_logger("robot_reach_study_node"), + "No source defined for path generation!"); + return false; + } + + if (!this->get_parameter("path_generation_config.db.config_name", + sp_.db_config_name) || + !this->get_parameter("path_generation_config.db.package", + sp_.db_package) || + !this->get_parameter("path_generation_config.db.directory", + sp_.db_dir) || + !this->get_parameter("path_generation_config.db.name", sp_.db_name)) { + return false; + } + + if (!this->get_parameter( + "path_generation_config.robot_configurations.name", + sp_.robot_configurations_name) || + !this->get_parameter( + "path_generation_config.robot_configurations.package", + sp_.robot_configurations_package) || + !this->get_parameter( + "path_generation_config.robot_configurations.directory", + sp_.robot_configurations_dir)) { + return false; + } + if (std::find(sp_.visualize_dbs.begin(), sp_.visualize_dbs.end(), "") != sp_.visualize_dbs.end()) { sp_.visualize_dbs.clear(); diff --git a/reach_core/src/utils/visualization_utils.cpp b/reach_core/src/utils/visualization_utils.cpp index 4d1f8b58..5e354406 100644 --- a/reach_core/src/utils/visualization_utils.cpp +++ b/reach_core/src/utils/visualization_utils.cpp @@ -60,62 +60,6 @@ visualization_msgs::msg::Marker makeVisual( marker.scale.y = scale / ARROW_SCALE_RATIO; marker.scale.z = scale / ARROW_SCALE_RATIO; - if (color) { - std::vector color_vec = *color; - marker.color.r = color_vec[0]; - marker.color.g = color_vec[1]; - marker.color.b = color_vec[2]; - marker.color.a = color_vec[3]; - } else { - marker.color.a = 1.0; // Don't forget to set the alpha! - - if (r.reached) { - marker.color.r = 1.0 - r.retrieved_fraction; - marker.color.g = 1.0 - r.retrieved_fraction; - marker.color.b = r.retrieved_fraction; - } else { - marker.color.r = 1.0; - marker.color.g = 0.0; - marker.color.b = 0.0; - } - } - - return marker; -} - -visualization_msgs::msg::Marker makeVisualTraj( - const rclcpp::Node::SharedPtr& node, const reach_msgs::msg::ReachRecord& r, - const std::string& frame, const double scale, const std::string& ns, - const boost::optional>& color) { - static int idx = 0; - - visualization_msgs::msg::Marker marker; - marker.header.frame_id = frame; - marker.header.stamp = node->now(); - marker.ns = ns; - marker.id = idx++; - marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; - marker.action = visualization_msgs::msg::Marker::ADD; - - // Convert back to geometry_msgs pose - geometry_msgs::msg::Pose msg; - marker.pose = msg; - - size_t wpts_size = r.waypoints.size(); - // fill the points - for (size_t i = 0; i < wpts_size && ((wpts_size % 7u) == 0u); i += 7u) { - geometry_msgs::msg::Point p_tmp; - p_tmp.set__x(r.waypoints[i + 0]); - p_tmp.set__y(r.waypoints[i + 1]); - p_tmp.set__z(r.waypoints[i + 2]); - marker.points.push_back(p_tmp); - } - - // sphere diameters - marker.scale.x = SPHERE_DIAMETER; - marker.scale.y = SPHERE_DIAMETER; - marker.scale.z = SPHERE_DIAMETER; - if (color) { std::vector color_vec = *color; marker.color.r = color_vec[0]; @@ -127,8 +71,8 @@ visualization_msgs::msg::Marker makeVisualTraj( if (r.reached) { marker.color.r = 0.0; - marker.color.g = 1.0; - marker.color.b = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; } else { marker.color.r = 1.0; marker.color.g = 0.0; @@ -155,12 +99,6 @@ visualization_msgs::msg::InteractiveMarker makeInteractiveMarker( // Visuals auto visual = utils::makeVisual(node, r, frame, scale); control.markers.push_back(visual); - // TODO (livanov93): what to do with this since trajectory fraction now is - // colorized by scaling between red and blue - // if (!r.waypoints.empty()) { - // auto visual_traj = utils::makeVisualTraj(node, r, frame, scale); - // control.markers.push_back(visual_traj); - // } m.controls.push_back(control); return m; diff --git a/reach_demo/config/params.yaml b/reach_demo/config/params.yaml index 37109b62..3ddce357 100644 --- a/reach_demo/config/params.yaml +++ b/reach_demo/config/params.yaml @@ -1,18 +1,36 @@ robot_reach_study_node: ros__parameters: - config_name: "demo_config" + + # moveit stuff + planning_group: "manipulator" fixed_frame: "base_link" object_frame: "reach_object" - results_package: "reach_demo" - results_directory: "results" + tool_frame: "tcp" + + # pcd stuff pcd_package: "reach_demo" pcd_filename_path: "config/part.pcd" + + # average neighbor calc get_avg_neighbor_count: false - compare_dbs: [""] + + # invert orientation of normals + invert_z_tool_rotation: false + + # db comparison + compare_dbs: [ "" ] + + # database stuff + config_name: "demo_config" + results_package: "reach_demo" + results_directory: "results" + + # to do path generation or not + generate_paths: true + visualize_results: true - planning_group: "manipulator" - run_initial_study_only: true keep_running: true + run_initial_study_only: false optimization: radius: 0.2 @@ -20,17 +38,10 @@ robot_reach_study_node: step_improvement_threshold: 0.01 ik_solver_config: - name: "moveit_reach_plugins/ik/CartesianRetrievalIKSolver" - max_velocity_scaling_factor: 0.5 - max_acceleration_scaling_factor: 0.5 -# name: "moveit_reach_plugins/ik/MoveItIKSolver" - retrieval_distance: 0.2 + name: "moveit_reach_plugins/ik/MoveItIKSolver" tool_frame: "tcp" - max_eef_step: 0.005 - jump_threshold: 0.1 # cca 5 degrees distance_threshold: 0.0 planning_group: "manipulator" -# collision_mesh_package: "reach_demo" collision_mesh_package: "package://reach_demo/config/part.ply" collision_mesh_filename_path: "config/part.ply" collision_mesh_frame: "reach_object" @@ -44,7 +55,6 @@ robot_reach_study_node: planning_group: "manipulator" distance_threshold: 0.025 exponent: 2 -# collision_mesh_package: "reach_demo" collision_mesh_package: "package://reach_demo/config/part.ply" collision_mesh_filename_path: "config/part.ply" collision_mesh_frame: "reach_object" @@ -53,9 +63,111 @@ robot_reach_study_node: display_config: name: "moveit_reach_plugins/display/MoveItReachDisplay" planning_group: "manipulator" -# collision_mesh_package: "reach_demo" collision_mesh_package: "package://reach_demo/config/part.ply" collision_mesh_filename_path: "config/part.ply" collision_mesh_frame: "reach_object" fixed_frame: "base_link" marker_scale: 0.05 + + path_generation_config: + + # one planning group for all path generation plugins + planning_group: "manipulator" + fixed_frame: "base_link" + + # initial sources are mutually exclusive + initial_source_db: true # false + db: + config_name: "demo_config" + package: "reach_demo" + directory: "results" + name: "optimized_reach.db" + + initial_source_robot_configurations: false # true + robot_configurations: + name: "source.yaml" + package: "reach_demo" + directory: "config" + + collision_mesh_package: "package://reach_demo/config/part.ply" + collision_mesh_frame: "reach_object" + + path_plugins: + - path_1 + - path_2 + # - path_3 + # - path_4 + + path_1: + plugin_name: "moveit_reach_plugins/path/CartesianPathGeneration" + ########################################################## + ############# PATH PLANNING SPECIFIC PARAMS ############## + ######################################################### + max_velocity_scaling_factor: 0.5 + max_acceleration_scaling_factor: 0.5 + tool_frame: "tcp" + max_eef_step: 0.005 + jump_threshold: 0.1 # cca 5 degrees + ######################################################### + distance_threshold: 0.0 # distance threshold for collision checking + global_transformation: + tx: 0.0 + ty: 0.0 + tz: 0.0 + rx: 0.0 #deg + ry: 0.0 #deg + rz: 0.0 #deg + tool_transformation: + tx: 0.0 + ty: 0.0 + tz: -0.2 + rx: 0.0 #deg + ry: 0.0 #deg + rz: 0.0 #deg + + path_2: + plugin_name: "moveit_reach_plugins/path/CartesianPathGeneration" + ########################################################## + ############# PATH PLANNING SPECIFIC PARAMS ############## + ######################################################### + max_velocity_scaling_factor: 0.5 + max_acceleration_scaling_factor: 0.5 + tool_frame: "tcp" + max_eef_step: 0.005 + jump_threshold: 0.1 # cca 5 degrees + ######################################################### + distance_threshold: 0.0 # distance threshold for collision checking + global_transformation: + tx: 0.0 + ty: 0.0 + tz: 0.0 + rx: 0.0 #deg + ry: 0.0 #deg + rz: 0.0 #deg + tool_transformation: + tx: 0.2 + ty: 0.0 + tz: 0.0 + rx: 0.0 #deg + ry: 0.0 #deg + rz: 0.0 #deg + + path_3: + plugin_name: "moveit_reach_plugins/path_generation/PlannerPath" + + path_4: + plugin_name: "moveit_reach_plugins/path_generation/CartesianPath" + global_transformation: + tx: 0.0 + ty: 0.0 + tz: 0.0 + rx: 0.0 #deg + ry: 0.0 #deg + rz: 0.0 #deg + tool_transformation: + tx: 0.0 + ty: 0.0 + tz: 0.0 + rx: 0.0 #deg + ry: 0.0 #deg + rz: 0.0 #deg diff --git a/reach_demo/config/path_generation.yaml b/reach_demo/config/path_generation.yaml new file mode 100644 index 00000000..46b9524f --- /dev/null +++ b/reach_demo/config/path_generation.yaml @@ -0,0 +1,146 @@ +robot_reach_study_node: + ros__parameters: + + # moveit stuff + planning_group: "manipulator" + fixed_frame: "base_link" + object_frame: "reach_object" + tool_frame: "tcp" + + # pcd stuff + pcd_package: "reach_demo" + pcd_filename_path: "config/part.pcd" + + # average neighbor calc + get_avg_neighbor_count: false + + # db comparison + compare_dbs: [ "" ] + + # database stuff + config_name: "demo_config" + results_package: "reach_demo" + results_directory: "results" + + # to do path generation or not + generate_paths: true + + visualize_results: true + keep_running: true + run_initial_study_only: false + + optimization: + radius: 0.2 + max_steps: 10 + step_improvement_threshold: 0.01 + + ik_solver_config: + name: "moveit_reach_plugins/ik/MoveItIKSolver" + tool_frame: "tcp" + distance_threshold: 0.0 + planning_group: "manipulator" + collision_mesh_package: "package://reach_demo/config/part.ply" + collision_mesh_filename_path: "config/part.ply" + collision_mesh_frame: "reach_object" + touch_links: [""] + evaluation_plugin: + name: "reach_core/plugins/MultiplicativeFactory" + plugins: ["moveit_reach_plugins/evaluation/ManipulabilityMoveIt", "moveit_reach_plugins/evaluation/DistancePenaltyMoveIt"] + moveit_reach_plugins/evaluation/ManipulabilityMoveIt: + planning_group: "manipulator" + moveit_reach_plugins/evaluation/DistancePenaltyMoveIt: + planning_group: "manipulator" + distance_threshold: 0.025 + exponent: 2 + collision_mesh_package: "package://reach_demo/config/part.ply" + collision_mesh_filename_path: "config/part.ply" + collision_mesh_frame: "reach_object" + touch_links: [""] + + display_config: + name: "moveit_reach_plugins/display/MoveItReachDisplay" + planning_group: "manipulator" + collision_mesh_package: "package://reach_demo/config/part.ply" + collision_mesh_filename_path: "config/part.ply" + collision_mesh_frame: "reach_object" + fixed_frame: "base_link" + marker_scale: 0.05 + + path_generation_config: + + # one planning group for all path generation plugins + planning_group: "manipulator" + fixed_frame: "base_link" + + # initial sources are mutually exclusive + initial_source_db: true # false + db: + config_name: "demo_config" + package: "reach_demo" + directory: "results" + name: "optimized_reach.db" + + initial_source_robot_configurations: false # true + robot_configurations: + name: "source.yaml" + package: "reach_demo" + directory: "config" + + collision_mesh_package: "package://reach_demo/config/part.ply" + collision_mesh_frame: "reach_object" + + path_plugins: + - path_1 +# - path_2 +# - path_3 +# - path_4 + + path_1: + plugin_name: "moveit_reach_plugins/path_generation/CartesianPath" + ########################################################## + ############# PATH PLANNING SPECIFIC PARAMS ############## + ######################################################### + max_velocity_scaling_factor: 0.5 + max_acceleration_scaling_factor: 0.5 + tool_frame: "tcp" + max_eef_step: 0.005 + jump_threshold: 0.1 # cca 5 degrees + ######################################################### + distance_threshold: 0.0 # distance threshold for collision checking + global_transformation: + tx: 0.0 + ty: 0.0 + tz: 0.0 + rx: 0.0 #deg + ry: 0.0 #deg + rz: 0.0 #deg + tool_transformation: + tx: 0.0 + ty: 0.0 + tz: 0.0 + rx: 0.0 #deg + ry: 0.0 #deg + rz: 0.0 #deg + + path_2: + plugin_name: "moveit_reach_plugins/path_generation/PlannerPath" + + path_3: + plugin_name: "moveit_reach_plugins/path_generation/PlannerPath" + + path_4: + plugin_name: "moveit_reach_plugins/path_generation/CartesianPath" + global_transformation: + tx: 0.0 + ty: 0.0 + tz: 0.0 + rx: 0.0 #deg + ry: 0.0 #deg + rz: 0.0 #deg + tool_transformation: + tx: 0.0 + ty: 0.0 + tz: 0.0 + rx: 0.0 #deg + ry: 0.0 #deg + rz: 0.0 #deg diff --git a/reach_demo/config/source.yaml b/reach_demo/config/source.yaml new file mode 100644 index 00000000..28e47d12 --- /dev/null +++ b/reach_demo/config/source.yaml @@ -0,0 +1,35 @@ +# starting robot configuration when path generation source is not database +robot_configurations: + - config_1 + - config_2 + - config_3 + +config_1: + joint_b: 0.0 + joint_e: 0.0 + joint_l: 0.0 + joint_r: 0.0 + joint_s: 0.0 + joint_t: 0.0 + joint_u: 0.0 + +config_2: + joint_b: 0.1 + joint_e: 0.1 + joint_l: 0.1 + joint_r: 0.1 + joint_s: 0.1 + joint_t: 0.1 + joint_u: 0.1 + +config_3: + joint_b: -0.1 + joint_e: -0.1 + joint_l: -0.1 + joint_r: -0.1 + joint_s: -0.1 + joint_t: -0.1 + joint_u: -0.1 + + + diff --git a/reach_msgs/CMakeLists.txt b/reach_msgs/CMakeLists.txt index a9004084..e37d0b5e 100644 --- a/reach_msgs/CMakeLists.txt +++ b/reach_msgs/CMakeLists.txt @@ -31,6 +31,7 @@ endforeach() set(msg_files "msg/ReachRecord.msg" "msg/ReachDatabase.msg" + "msg/ReachPath.msg" ) set(srv_files diff --git a/reach_msgs/msg/ReachPath.msg b/reach_msgs/msg/ReachPath.msg new file mode 100644 index 00000000..d1039d5d --- /dev/null +++ b/reach_msgs/msg/ReachPath.msg @@ -0,0 +1,6 @@ +# An individual trajectory id +string id +# fraction of desired path +float64 fraction +# trajectory +moveit_msgs/RobotTrajectory moveit_trajectory diff --git a/reach_msgs/msg/ReachRecord.msg b/reach_msgs/msg/ReachRecord.msg index 523dfc78..d4b4e323 100644 --- a/reach_msgs/msg/ReachRecord.msg +++ b/reach_msgs/msg/ReachRecord.msg @@ -5,12 +5,5 @@ bool reached sensor_msgs/JointState goal_state sensor_msgs/JointState seed_state float64 score -# needed for path computation -float64 retrieved_fraction -# in packets of size goal_state.name.size() -float64[] joint_space_trajectory -# each has x,y,z qx,qy,qz,qw -float64[] waypoints -# ik solver name -string ik_solver -moveit_msgs/RobotTrajectory moveit_trajectory +# all the paths that start from this reach point +ReachPath[] paths