From 00278608306ac29b3278821dd5892412dc0300c8 Mon Sep 17 00:00:00 2001 From: Nicolas Hiillos Date: Tue, 14 Feb 2023 14:33:06 +0200 Subject: [PATCH] Fix planning failure resulting in crash --- .../launch/lite6_real.launch.py | 18 ++++++- src/lite6_controller/src/lite6_controller.cpp | 53 +++++++++++-------- 2 files changed, 48 insertions(+), 23 deletions(-) diff --git a/src/lite6_controller/launch/lite6_real.launch.py b/src/lite6_controller/launch/lite6_real.launch.py index 77a9c57..3a0e065 100644 --- a/src/lite6_controller/launch/lite6_real.launch.py +++ b/src/lite6_controller/launch/lite6_real.launch.py @@ -171,6 +171,11 @@ def launch_setup(context, *args, **kwargs): if joint_limits_yaml and gripper_joint_limits_yaml: joint_limits_yaml['joint_limits'].update(gripper_joint_limits_yaml['joint_limits']) + # FIX acceleration limits + for i in range(1,7): + joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True + joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 0.5 + add_prefix_to_moveit_params = getattr(mod, 'add_prefix_to_moveit_params') add_prefix_to_moveit_params( controllers_yaml=controllers_yaml, ompl_planning_yaml=ompl_planning_yaml, @@ -222,6 +227,16 @@ def launch_setup(context, *args, **kwargs): # }, } + + pilz_planning_pipeline_config = { + 'move_group': { + '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""", + "default_planner_config": "PTP", + "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService", + } + } + # Start the actual move_group node/action server move_group_node = Node( package='moveit_ros_move_group', @@ -229,7 +244,8 @@ def launch_setup(context, *args, **kwargs): output='screen', parameters=[ robot_description_parameters, - ompl_planning_pipeline_config, + #ompl_planning_pipeline_config, + pilz_planning_pipeline_config, trajectory_execution, plan_execution, moveit_controllers, diff --git a/src/lite6_controller/src/lite6_controller.cpp b/src/lite6_controller/src/lite6_controller.cpp index 9571c2f..96f1a64 100644 --- a/src/lite6_controller/src/lite6_controller.cpp +++ b/src/lite6_controller/src/lite6_controller.cpp @@ -170,12 +170,12 @@ public: // // Set limits for A4 paper - float xlim_lower = 0.12; - float xlim_upper = 0.2; - float ylim_lower = -0.2; - float ylim_upper = 0.2; - float zlim_lower = 0.1; - float zlim_upper = 0.2; + float xlim_lower = 0.20; + float xlim_upper = 0.40; + float ylim_lower = -0.20; + float ylim_upper = 0.20; + float zlim_lower = 0.145; + float zlim_upper = 0.16; /** * Function that translates an input value with a given range to a value within another range. @@ -194,8 +194,11 @@ public: geometry_msgs::msg::PoseStamped translatePose(geometry_msgs::msg::PoseStamped pose) { // TODO support paper angle - pose.pose.position.x = translate(pose.pose.position.x, 0, 1, xlim_lower, xlim_upper); - pose.pose.position.y = translate(pose.pose.position.y, 0, 1, ylim_lower, ylim_upper); + auto x = pose.pose.position.x; + auto y = pose.pose.position.y; + // X and Y are swapped in xArm space + pose.pose.position.x = translate(y, 0, 1, xlim_lower, xlim_upper); + pose.pose.position.y = translate(x, 0, 1, ylim_lower, ylim_upper); pose.pose.position.z = translate(pose.pose.position.z, 0, 1, zlim_lower, zlim_upper); return pose; @@ -211,10 +214,12 @@ public: /** * Creates a trajectory for a pose and appends it to a given trajectory */ - void addPoseToTrajectory(geometry_msgs::msg::PoseStamped pose, moveit_msgs::msg::RobotTrajectory *trajectory) + bool addPoseToTrajectory(geometry_msgs::msg::PoseStamped pose, moveit_msgs::msg::RobotTrajectory *trajectory) { pose = translatePose(pose); - move_group.setPoseTarget(pose); + //move_group.setPoseTarget(pose); + move_group.setApproximateJointValueTarget(pose, "link_eef"); + //moveit_msgs::msg::RobotTrajectory trajectory; //move_group.setPlanningPipelineId("PTP"); move_group.setPlannerId("PTP"); @@ -227,21 +232,25 @@ public: bool success = (move_group.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS); RCLCPP_INFO(this->get_logger(), "Plan (pose goal) %s", success ? "SUCCEEDED" : "FAILED"); - robot_trajectory::RobotTrajectory next_trajectory(move_group.getCurrentState()->getRobotModel(), move_group.getName()); - next_trajectory.setRobotTrajectoryMsg(*move_group.getCurrentState(), plan.trajectory_); + if (success) + { + robot_trajectory::RobotTrajectory next_trajectory(move_group.getCurrentState()->getRobotModel(), move_group.getName()); + next_trajectory.setRobotTrajectoryMsg(*move_group.getCurrentState(), plan.trajectory_); - // append trajectory, with time step of 2.0, not skipping any points - previous_trajectory.append(next_trajectory, 2.0, 0); - *trajectory = moveit_msgs::msg::RobotTrajectory(); - previous_trajectory.getRobotTrajectoryMsg(*trajectory); + // append trajectory, with time step of 2.0, not skipping any points + previous_trajectory.append(next_trajectory, 0.01, 0); + *trajectory = moveit_msgs::msg::RobotTrajectory(); + previous_trajectory.getRobotTrajectoryMsg(*trajectory); - // Append segment to complete trajectory - //trajectory->joint_trajectory.points.insert(trajectory->joint_trajectory.points.end(), - // plan.trajectory_.joint_trajectory.points.begin(), - // plan.trajectory_.joint_trajectory.points.end()); - //trajectory->joint_trajectory.joint_names = plan.trajectory_.joint_trajectory.joint_names; + // Append segment to complete trajectory + //trajectory->joint_trajectory.points.insert(trajectory->joint_trajectory.points.end(), + // plan.trajectory_.joint_trajectory.points.begin(), + // plan.trajectory_.joint_trajectory.points.end()); + //trajectory->joint_trajectory.joint_names = plan.trajectory_.joint_trajectory.joint_names; + } move_group.clearPoseTarget(); + return success; } /** @@ -286,7 +295,7 @@ public: //RCLCPP_INFO(this->get_logger(), "Planning trajectory"); // Append next pose to trajectory - addPoseToTrajectory(p, &multi_trajectory); + if (!addPoseToTrajectory(p, &multi_trajectory)) continue; // set move_group start state to final pose of trajectory //RCLCPP_INFO(this->get_logger(), "setRobotTrajectoryMsg");