From e0d84b2adcc6d043e4ac3dee1c4a31561e4605de Mon Sep 17 00:00:00 2001 From: Sravan Balaji Date: Sun, 26 Apr 2020 14:36:06 -0400 Subject: [PATCH 1/2] Arm Controller Retry Attempts & Intermediate Positions Co-authored-by: dwitcpa - Add parameters for arm controller: time per step, retry attempts, and z adjustment when trying to attach item - Implement retry attempts for arm_destination_callback - Add enum arm_action_phase to allow for different trajectories based on phase - Add intermediate positions to send_joint_trajectory function - gripper_control function now returns success status --- .../src/rrrobot/src/arm_controller_node.cpp | 192 +++++++++++++----- .../src/rrrobot/src/rrrobot_node.cpp | 8 +- 2 files changed, 145 insertions(+), 55 deletions(-) diff --git a/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp b/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp index 0c3ae39..9948b0f 100644 --- a/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp @@ -6,6 +6,7 @@ #include #include +#include #include #include @@ -32,8 +33,6 @@ #include #include -#include - #include using namespace std; @@ -42,7 +41,7 @@ using namespace std; class ArmController { public: - ArmController(ros::NodeHandle &node, ArmRepresentation *arm_) : gripper_enabled_(false), item_attached_(false), arm(arm_) + ArmController(ros::NodeHandle &node, ArmRepresentation *arm_, double time_per_step, int retry_attempts, double item_attach_z_adjustment) : gripper_enabled_(false), item_attached_(false), arm(arm_), time_per_step(time_per_step), retry_attempts(retry_attempts), item_attach_z_adjustment(item_attach_z_adjustment) { arm_joint_trajectory_publisher_ = node.advertise(ARM_COMMAND_CHANNEL, 10); @@ -103,65 +102,106 @@ public: void arm_destination_callback(const rrrobot::arm_command &target_pose) { ROS_INFO("Received target pose"); - + int attempts = 0; int nbr_joints = arm->getChain()->getNrOfJoints(); vector positions; - double time_from_start = 5; // Seconds - ros::Rate timeout(1.0/time_from_start);// + + // Turn on suction + while (!gripper_control(true)) { + if (attempts == retry_attempts) { + ROS_ERROR("STEP 1: Suction failed, not going to target"); + return; + } + + ros::Duration(1).sleep(); + attempts++; + } + + ROS_INFO("STEP 1: Suction turned on"); // Move arm to pickup item from conveyor belt + attempts = 1; positions = calc_joint_positions(target_pose.grab_location); - send_joint_trajectory(positions, time_from_start); + send_joint_trajectory(positions, arm_action_phase::bin_to_belt); - // Wait until target position is reached - while (!have_reached_target(frame_to_pose(calc_end_effector_pose()), target_pose.grab_location)) - { - // TODO: Do something if not able to reach target - time_from_start += 5; - send_joint_trajectory(positions, time_from_start); - timeout.sleep(); - - // update sleep time_from_start - timeout = ros::Rate(1.0/time_from_start); - continue; + // Check if target has been reached + while(!have_reached_target(frame_to_pose(calc_end_effector_pose()), target_pose.grab_location)) { + if (attempts == retry_attempts) { + ROS_ERROR("STEP 2: Did not reach target in allotted time"); + return; + } + + send_joint_trajectory(positions, arm_action_phase::bin_to_belt); + attempts++; } - // Turn on suction - while (!gripper_enabled_) - { - gripper_control(true); - } + ROS_INFO("STEP 2: Reached target conveyor belt position"); + + attempts = 0; + geometry_msgs::Pose target_location = target_pose.grab_location; // Wait until object is attached while (!item_attached_) { - // TODO: Do something if item is not able to attach - continue; + if (attempts == retry_attempts) { + ROS_ERROR("STEP 3: Could not pick up item"); + return; + } + + // If item is not able to attach, adjust z and try again + target_location.position.z -= item_attach_z_adjustment; // meters + positions = calc_joint_positions(target_location); + send_joint_trajectory(positions, arm_action_phase::item_pickup_adjustment); + attempts++; } + ROS_INFO("STEP 3: Item picked up"); + // Move item to desired position + attempts = 1; positions = calc_joint_positions(target_pose.drop_location); - send_joint_trajectory(positions, time_from_start); + send_joint_trajectory(positions, arm_action_phase::belt_to_bin); - // Wait until target position is reached - while (!have_reached_target(frame_to_pose(calc_end_effector_pose()), target_pose.drop_location)) - { - // TODO: Do something if not able to reach target - continue; + // Check if target has been reached + while(!have_reached_target(frame_to_pose(calc_end_effector_pose()), target_pose.grab_location)) { + if (attempts == retry_attempts) { + ROS_ERROR("STEP 4: Did not reach target in allotted time"); + return; + } + + send_joint_trajectory(positions, arm_action_phase::belt_to_bin); + attempts++; } + ROS_INFO("STEP 4: Reached drop location"); + // Turn off suction - while (gripper_enabled_) - { - gripper_control(false); + attempts = 0; + while (!gripper_control(false)) { + if (attempts == retry_attempts) { + ROS_ERROR("STEP 5: Suction not turning off... ¯\\_(ツ)_/¯"); + return; + } + + ros::Duration(1).sleep(); + attempts++; } + ROS_INFO("STEP 5: Suction turned off"); + // Wait until object is detached + attempts = 0; while (item_attached_) { - // TODO: Do something if item doesn't detach - continue; + if (attempts == retry_attempts) { + ROS_ERROR("STEP 6: Item not detaching.. ¯\\_(ツ)_/¯"); + } + + ros::Duration(0.1).sleep(); + attempts++; } + + ROS_INFO("STEP 6: Dropped item in bin"); } private: @@ -171,6 +211,14 @@ private: ros::ServiceClient gripper_; KDL::JntArray arm_current_joint_states_; ArmRepresentation *arm; + enum arm_action_phase { + belt_to_bin, + bin_to_belt, + item_pickup_adjustment + }; + double time_per_step; + int retry_attempts; + double item_attach_z_adjustment; vector calc_joint_positions(const geometry_msgs::Pose &pose) { @@ -261,38 +309,75 @@ private: return pose; } - void send_joint_trajectory(const vector &positions, double time_from_start) + void send_joint_trajectory(const vector &target, arm_action_phase phase) { // Declare JointTrajectory message trajectory_msgs::JointTrajectory msg; + // Define intermediate joint positions + const vector above_conveyor = {0, 0, -M_PI/2, M_PI/2, -M_PI/2, -M_PI/2, 0}; + const vector above_bins = {0, M_PI, -M_PI/2, M_PI/2, -M_PI/2, -M_PI/2, 0}; + // Fill the names of the joints to be controlled msg.joint_names = arm->get_joint_names(); - // Create one point in the trajectory - msg.points.resize(1); - msg.points[0].positions.resize(msg.joint_names.size(), 0.0); + int num_points = 3; - // Set joint positions - for (size_t idx = 0; idx < positions.size(); ++idx) - { - msg.points[0].positions[idx] = positions[idx]; + // Create points in the trajectory + msg.points.resize(num_points); + + for (int i = 0; i < num_points; ++i) { + if (i == 0) { + if (phase == arm_action_phase::belt_to_bin) { + msg.points[i].positions = above_conveyor; + } + else if (phase == arm_action_phase::bin_to_belt) { + msg.points[i].positions = above_bins; + } + else if (phase == arm_action_phase::item_pickup_adjustment) { + num_points = 1; + msg.points.resize(num_points); + msg.points[i].positions = target; + msg.points[i].time_from_start = ros::Duration(time_per_step); + break; + } + + msg.points[i].time_from_start = ros::Duration((i+1)*time_per_step); + } + else if (i == 1) { + if (phase == arm_action_phase::belt_to_bin) { + msg.points[i].positions = above_bins; + } + else if (phase == arm_action_phase::bin_to_belt) { + msg.points[i].positions = above_conveyor; + } + + msg.points[i].time_from_start = ros::Duration((i+1)*time_per_step); + } + else if (i == (num_points - 1)) { + msg.points[i].positions = target; + msg.points[i].time_from_start = ros::Duration((i+1)*time_per_step); + } } - // How long to take getting to the point (floating point seconds) - msg.points[0].time_from_start = ros::Duration(time_from_start); - - //ROS_INFO("Sending command: \n%s", msg); - arm_joint_trajectory_publisher_.publish(msg); + + // Wait to reach target + ros::Duration((num_points+1)*time_per_step).sleep(); } - void gripper_control(bool state) + bool gripper_control(bool state) { osrf_gear::VacuumGripperControl srv; srv.request.enable = state; - gripper_.call(srv); + bool success = gripper_.call(srv); + + if(!success) { + ROS_ERROR("Could not enable gripper"); + } + + return success; } bool have_reached_target(geometry_msgs::Pose cur, geometry_msgs::Pose target) @@ -350,7 +435,10 @@ int main(int argc, char **argv) ArmRepresentation arm; - ArmController ac(node, &arm); + double time_per_step = 3.0; // seconds + int retry_attempts = 3; + double item_attach_z_adjustment = 0.025; // meters + ArmController ac(node, &arm, time_per_step, retry_attempts, item_attach_z_adjustment); // Subscribe to arm destination and joint states channels ros::Subscriber arm_destination_sub = node.subscribe(ARM_DESTINATION_CHANNEL, 1, &ArmController::arm_destination_callback, &ac); diff --git a/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp b/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp index e0c9fc4..da7bad4 100644 --- a/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp @@ -49,9 +49,9 @@ public: class RRRobot { public: - RRRobot() + RRRobot(ros::NodeHandle &node) : current_robot_state(RobotState::WAITING_FOR_CLASSIFICATION | RobotState::WAITING_FOR_GRAB_LOCATION), - nh() + nh(node) { cv_classification_sub = nh.subscribe(CV_CLASSIFICATION_CHANNEL, 1000, &RRRobot::cv_classification_callback, this); gripper_state_sub = nh.subscribe(GRIPPER_STATE_CHANNEL, 1000, &RRRobot::gripper_state_callback, this); @@ -201,7 +201,9 @@ int main(int argc, char **argv) { ros::init(argc, argv, "rrrobot"); - RRRobot robot; + ros::NodeHandle node; + + RRRobot robot(node); ros::spin(); // This executes callbacks on new data until ctrl-c. return 0; From ed45f76ae087fcc1aa56a5799d10cbccd84f4d00 Mon Sep 17 00:00:00 2001 From: Sravan Balaji Date: Sun, 26 Apr 2020 15:03:59 -0400 Subject: [PATCH 2/2] Tune target reached thresholds - Add QuatToEuler function to convert quaternion to roll, pitch, yaw for easy angle threshold comparison - Tune position and rotation thresholds --- .../src/rrrobot/src/arm_controller_node.cpp | 34 +++++++++++-------- 1 file changed, 20 insertions(+), 14 deletions(-) diff --git a/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp b/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp index 9948b0f..2e7b0ca 100644 --- a/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp @@ -34,6 +34,7 @@ #include #include +#include using namespace std; @@ -383,8 +384,8 @@ private: bool have_reached_target(geometry_msgs::Pose cur, geometry_msgs::Pose target) { // TODO: Tune threshold values - float pos_thresh = 0.01; // Meters - float rot_thresh = 0.02; // Radians + float pos_thresh = 0.1; // Meters + float rot_thresh = 0.0875; // Radians float pos_err = fabs(cur.position.x - target.position.x) + fabs(cur.position.y - target.position.y) + @@ -395,33 +396,38 @@ private: return false; } - float qx_err = fabs(cur.orientation.x - target.orientation.x); - float qy_err = fabs(cur.orientation.y - target.orientation.y); - float qz_err = fabs(cur.orientation.z - target.orientation.z); - float qw_err = fabs(cur.orientation.w - target.orientation.w); + vector rpy1 = QuatToEuler(cur.orientation); + vector rpy2 = QuatToEuler(target.orientation); - if (qx_err > rot_thresh) + float rx_err = fabs(fmod(rpy1[0] - rpy2[0], 2*M_PI)); + float ry_err = fabs(fmod(rpy1[1] - rpy2[1], 2*M_PI)); + float rz_err = fabs(fmod(rpy1[2] - rpy2[2], 2*M_PI)); + + if (rx_err > rot_thresh) { return false; } - if (qy_err > rot_thresh) + if (ry_err > rot_thresh) { return false; } - if (qz_err > rot_thresh) - { - return false; - } - - if (qw_err > rot_thresh) + if (rz_err > rot_thresh) { return false; } return true; } + + vector QuatToEuler(geometry_msgs::Quaternion q) { + KDL::Rotation r = KDL::Rotation::Quaternion(q.x, q.y, q.z, q.w); + vector rpy = {0, 0, 0}; + r.getRPY(rpy[0], rpy[1], rpy[2]); + + return rpy; + } }; int main(int argc, char **argv)