diff --git a/twc_application/include/twc_application/raster_applicataion.h b/twc_application/include/twc_application/raster_applicataion.h index 7e74397..07c638d 100644 --- a/twc_application/include/twc_application/raster_applicataion.h +++ b/twc_application/include/twc_application/raster_applicataion.h @@ -36,6 +36,7 @@ class RasterApplication : public Application // Get global parameter use_rail_ = nh.param("/use_rail", use_rail_); + use_positioner_ = nh.param("/use_positioner", use_positioner_); } void run() override @@ -49,19 +50,20 @@ class RasterApplication : public Application Eigen::Isometry3d post_transform = Eigen::Isometry3d::Identity() * Eigen::Translation3d(0, 0, 0.05) * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()); - Eigen::Isometry3d pre_transform = current_transforms["part_link"]; + Eigen::Isometry3d pre_transform = Eigen::Isometry3d::Identity(); tesseract_common::Toolpath paths = parsePathFromFile(toolpath_, pre_transform, post_transform); tesseract_common::Toolpath raster_strips = filterPath(paths); std::string motion_group; - if (use_rail_) + if (use_rail_ || use_positioner_) motion_group = "manipulator"; else motion_group = "robot_only"; tesseract_planning::ManipulatorInfo manip_info(motion_group); manip_info.tcp = tesseract_planning::ToolCenterPoint("st_tool0"); + manip_info.working_frame = "part_link"; tesseract_planning::CompositeInstruction program = createProgram(manip_info, raster_strips); @@ -108,6 +110,13 @@ class RasterApplication : public Application freespace_profile = "FREESPACE_RAIL"; joint_names = { "rail_joint_1", "robot_joint_1", "robot_joint_2", "robot_joint_3", "robot_joint_4", "robot_joint_5", "robot_joint_6" }; } + else if (use_positioner_) + { + raster_profile = "RASTER_POSITIONER"; + transition_profile = "TRANSITION_POSITIONER"; + freespace_profile = "FREESPACE_POSITIONER"; + joint_names = { "positioner_base_joint", "positioner_joint_1", "robot_joint_1", "robot_joint_2", "robot_joint_3", "robot_joint_4", "robot_joint_5", "robot_joint_6" }; + } tesseract_planning::CompositeInstruction program("raster_program", tesseract_planning::CompositeInstructionOrder::ORDERED, manip_info); @@ -173,6 +182,7 @@ class RasterApplication : public Application private: std::string toolpath_; bool use_rail_ {false}; + bool use_positioner_ {false}; actionlib::SimpleActionClient ac_; }; diff --git a/twc_application/launch/application_positioner.launch b/twc_application/launch/application_positioner.launch index 859cfff..96fcc3a 100644 --- a/twc_application/launch/application_positioner.launch +++ b/twc_application/launch/application_positioner.launch @@ -3,6 +3,7 @@ + diff --git a/twc_motion_planning/include/twc_motion_planning/utils.h b/twc_motion_planning/include/twc_motion_planning/utils.h index d693b80..fea24a9 100644 --- a/twc_motion_planning/include/twc_motion_planning/utils.h +++ b/twc_motion_planning/include/twc_motion_planning/utils.h @@ -3,7 +3,7 @@ #include #include -#include +#include namespace twc { @@ -16,10 +16,7 @@ enum class ProfileType inline bool isRobotConfigValid(tesseract_planning::RobotConfig config) { - if (config != tesseract_planning::RobotConfig::NUT && config != tesseract_planning::RobotConfig::FUT) - return false; - - return true; + return (config == tesseract_planning::RobotConfig::NUT || config == tesseract_planning::RobotConfig::FUT); } inline bool isRobotConfigValid(tesseract_planning::RobotConfig start_config, tesseract_planning::RobotConfig end_config) @@ -72,17 +69,17 @@ class RobotConfigEdgeEvaluator : public descartes_light::EdgeEvaluator evaluate(const Eigen::Matrix& start, - const Eigen::Matrix& end) const override + std::pair evaluate(const descartes_light::State& start, + const descartes_light::State& end) const override { // Consider the edge: Eigen::Vector2i sign_correction(-1, 1); - auto start_config = tesseract_planning::getRobotConfig(robot_only_kin_, start.tail(robot_only_kin_->numJoints()), sign_correction); - auto end_config = tesseract_planning::getRobotConfig(robot_only_kin_, end.tail(robot_only_kin_->numJoints()), sign_correction); + auto start_config = tesseract_planning::getRobotConfig(robot_only_kin_, start.values.tail(robot_only_kin_->numJoints()), sign_correction); + auto end_config = tesseract_planning::getRobotConfig(robot_only_kin_, end.values.tail(robot_only_kin_->numJoints()), sign_correction); if (isRobotConfigValid(start_config, end_config)) return std::make_pair(true, FloatType(0)); @@ -100,8 +97,8 @@ class WeightedEuclideanDistanceEdgeEvaluator : public descartes_light::EdgeEvalu public: WeightedEuclideanDistanceEdgeEvaluator(const Eigen::VectorXd& weights) : weights_(weights.cast()) {} - std::pair evaluate(const Eigen::Matrix& start, - const Eigen::Matrix& end) const override + std::pair evaluate(const descartes_light::State& start, + const descartes_light::State& end) const override { FloatType cost = (weights_.array() * (end - start).array().abs()).sum(); return std::make_pair(true, cost); diff --git a/twc_motion_planning/src/twc_planning_server_node.cpp b/twc_motion_planning/src/twc_planning_server_node.cpp index 3653d0a..2c3fb5d 100644 --- a/twc_motion_planning/src/twc_planning_server_node.cpp +++ b/twc_motion_planning/src/twc_planning_server_node.cpp @@ -36,7 +36,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include diff --git a/twc_support/config/workcell_positioner.srdf b/twc_support/config/workcell_positioner.srdf index 3e10110..c07cb78 100644 --- a/twc_support/config/workcell_positioner.srdf +++ b/twc_support/config/workcell_positioner.srdf @@ -7,9 +7,23 @@ - + + + + + + + + + + + + + + +