nic/drawing-robot-ros2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixes

Browse Source
This commit is contained in:
Nicolas Hiillos
2023-03-01 07:43:44 +02:00
parent fc957ac078
commit dd7893e207
2 changed files with 12 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/lite6_controller/launch/lite6_gazebo.launch.py
View File

@@ -369,7 +369,9 @@ def launch_setup(context, *args, **kwargs):
pilz_planning_pipeline_config = { pilz_planning_pipeline_config = {
'move_group': { 'move_group': {
'planning_plugin': 'pilz_industrial_motion_planner/CommandPlanner', 'planning_plugin': 'pilz_industrial_motion_planner/CommandPlanner',
'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", # Disable AddTimeOptimalParameterization to fix motion blending https://github.com/ros-planning/moveit/issues/2905
'request_adapters': """default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
#'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
"default_planner_config": "PTP", "default_planner_config": "PTP",
} }
} }

14
src/lite6_controller/src/lite6_controller.cpp
View File

@@ -372,13 +372,19 @@ public:
//waypoints.push_back(move_group.getCurrentPose().pose); //waypoints.push_back(move_group.getCurrentPose().pose);
std::string ee_link = move_group.getLinkNames().back(); std::string ee_link = move_group.getLinkNames().back();
RCLCPP_INFO(this->get_logger(), "Got ee_link: %s", ee_link.c_str()); RCLCPP_INFO(this->get_logger(), "Got ee_link: %s", ee_link.c_str());
long n = 0;
for (auto p : goal->motion.path) for (auto p : goal->motion.path)
{ {
//if (n <= 0) continue;
//else n--;
//RCLCPP_INFO(this->get_logger(), "Creating MSI"); //RCLCPP_INFO(this->get_logger(), "Creating MSI");
moveit_msgs::msg::MotionSequenceItem msi = moveit_msgs::msg::MotionSequenceItem(); moveit_msgs::msg::MotionSequenceItem msi = moveit_msgs::msg::MotionSequenceItem();
planning_interface::MotionPlanRequest mpr = planning_interface::MotionPlanRequest(); planning_interface::MotionPlanRequest mpr = planning_interface::MotionPlanRequest();
mpr.planner_id = "PTP"; mpr.planner_id = "PTP";
mpr.group_name = move_group.getName();
mpr.max_velocity_scaling_factor = 1.0;
mpr.max_acceleration_scaling_factor = 1.0;
mpr.allowed_planning_time = 100; mpr.allowed_planning_time = 100;
mpr.max_cartesian_speed = 1; // m/s mpr.max_cartesian_speed = 1; // m/s
//mpr.goal_constraints.position_constraints.header.frame_id = "world"; //mpr.goal_constraints.position_constraints.header.frame_id = "world";
@@ -394,21 +400,18 @@ public:
//RCLCPP_INFO(this->get_logger(), "Got ee_link"); //RCLCPP_INFO(this->get_logger(), "Got ee_link");
mpr.group_name = MOVE_GROUP;
//moveit_msgs::msg::Constraints pose_goal = //moveit_msgs::msg::Constraints pose_goal =
// kinematic_constraints::constructGoalConstraints(ee_link, p, tolerance_pose, tolerance_angle); // kinematic_constraints::constructGoalConstraints(ee_link, p, tolerance_pose, tolerance_angle);
//kinematic_constraints::constructGoalConstraints(ee_link, pose, tolerance_pose, tolerance_angle); //kinematic_constraints::constructGoalConstraints(ee_link, pose, tolerance_pose, tolerance_angle);
moveit_msgs::msg::Constraints pose_goal = moveit_msgs::msg::Constraints pose_goal =
kinematic_constraints::constructGoalConstraints(ee_link, p, 1.0, 1.0); kinematic_constraints::constructGoalConstraints(ee_link, translatePose(p), 1e-6, 1e-6);
//kinematic_constraints::constructGoalConstraints(ee_link, p, 1.0, 1.0); //kinematic_constraints::constructGoalConstraints(ee_link, p, 1.0, 1.0);
//kinematic_constraints::constructGoalConstraints(ee_link, p, 1e-3, 1e-2); //kinematic_constraints::constructGoalConstraints(ee_link, p, 1e-3, 1e-2);
mpr.goal_constraints.push_back(pose_goal); mpr.goal_constraints.push_back(pose_goal);
mpr.max_velocity_scaling_factor = 1.0;
mpr.max_acceleration_scaling_factor = 1.0;
msi.req = mpr; msi.req = mpr;
msi.blend_radius = 0.001; //TODO make configurable msi.blend_radius = 0.0001; //TODO make configurable
msr.items.push_back(msi); msr.items.push_back(msi);
} }
msr.items.back().blend_radius = 0.0; // Last element blend must be 0 msr.items.back().blend_radius = 0.0; // Last element blend must be 0
@@ -429,6 +432,7 @@ public:
RCLCPP_INFO(this->get_logger(), "Sending request to sequence service"); RCLCPP_INFO(this->get_logger(), "Sending request to sequence service");
auto res = sequence_client_->async_send_request(req); auto res = sequence_client_->async_send_request(req);
// Wait for the result. // Wait for the result.
RCLCPP_INFO(this->get_logger(), "Waiting for result");
res.wait(); res.wait();
try try
{ {