diff --git a/src/rrrobot_ws/src/rrrobot/include/arm_representation.h b/src/rrrobot_ws/src/rrrobot/include/arm_representation.h index 98dbdb4..fc8f129 100644 --- a/src/rrrobot_ws/src/rrrobot/include/arm_representation.h +++ b/src/rrrobot_ws/src/rrrobot/include/arm_representation.h @@ -1,4 +1,4 @@ -// arm_controller_node.cpp +// arm_representation.h #include #include @@ -56,4 +56,4 @@ private: // KDL::ChainFkSolverPos_recursive fk_solver; // KDL::ChainIkSolverPos_LMA ik_solver; -}; \ No newline at end of file +}; diff --git a/src/rrrobot_ws/src/rrrobot/include/topic_names.h b/src/rrrobot_ws/src/rrrobot/include/topic_names.h index 4d6e3cd..de17340 100644 --- a/src/rrrobot_ws/src/rrrobot/include/topic_names.h +++ b/src/rrrobot_ws/src/rrrobot/include/topic_names.h @@ -1,3 +1,5 @@ +// topic_names.h + // COMPUTER VISION #define CV_CLASSIFICATION_CHANNEL "/cv_model" // CV Model classifications @@ -13,4 +15,4 @@ // COMPETITION #define START_COMPETITION_CHANNEL "/ariac/start_competition" // Start ARIAC competition -#define CONVEYOR_POWER_CHANNEL "/ariac/conveyor/control" // Turn conveyor belt on or off \ No newline at end of file +#define CONVEYOR_POWER_CHANNEL "/ariac/conveyor/control" // Turn conveyor belt on or off 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 53949a3..c6c0d7f 100644 --- a/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp @@ -45,6 +45,7 @@ using namespace std; class ArmController { public: + // Constructor 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); @@ -55,28 +56,24 @@ public: /* initialize random seed: */ srand(time(NULL)); - - // arm = std::move(arm_); - // ROS_INFO("Arm size (in constructor): %d", arm->getChain()->getNrOfJoints()); - // arm_current_joint_states_ = KDL::JntArray(arm->getChain()->getNrOfJoints()); - // KDL::SetToZero(arm_current_joint_states_); } + // Receive joint state messages void joint_state_callback(const sensor_msgs::JointState &joint_state_msg) { - // ROS_INFO("Received joint state"); - - // Convert std_msgs::double[] to KDL::JntArray + // Copy joint states to private variable arm_current_joint_states_ int nbr_joints = arm->getChain()->getNrOfJoints(); vector msg_joint_names = joint_state_msg.name; vector cur_joint_names = arm->get_joint_names(); vector position = joint_state_msg.position; + // Set all values to 0 for (int i = 0; i < nbr_joints; ++i) { arm_current_joint_states_[i] = 0.0; } + // Match up joint names from message to internal joint names order for (size_t i = 0; i < position.size(); ++i) { for (size_t j = 0; j < cur_joint_names.size(); ++j) @@ -89,12 +86,15 @@ public: } } + // Receive gripper state messages void gripper_state_callback(const osrf_gear::VacuumGripperState::ConstPtr &gripper_state_msg) { + // Store message states in private variables gripper_enabled_ = gripper_state_msg->enabled; item_attached_ = gripper_state_msg->attached; } + // Receive message from RRRobot node with desired pose to pick up object from conveyor belt void arm_destination_callback(const rrrobot::arm_command &target_pose) { ROS_INFO("Received target pose"); @@ -161,9 +161,8 @@ public: // Move item to desired position attempts = 1; - // if (calc_joint_positions(target_pose.drop_location, above_bins, positions)) - // send_joint_trajectory(positions, arm_action_phase::belt_to_bin); KDL::Frame end_effector_pose; + if (target_pose.drop_location.position.y > 1.0) { positions = bin1; @@ -194,7 +193,6 @@ public: return; } - // if (calc_joint_positions(target_pose.grab_location, get_randomized_start_state(above_bins) /*target_pose.drop_location.position.y)*/, positions)) send_joint_trajectory(positions, arm_action_phase::belt_to_bin); attempts++; } @@ -256,26 +254,24 @@ private: const vector bin1 = {0.05, 0.0, -2.2, -2.2, -0.25, 1.5708, 0.0}; const vector bin2 = {-0.7, 0.0, -2.2, -2.2, -0.25, 1.5708, 0.0}; + // Randomize start state to avoid inverse kinematics getting stuck in local minimum vector get_randomized_start_state(const vector &preferred_state) { - //int state_to_return = rand() % 2; - - // if (state == 0) - // return preferred_state; - vector cur; std::stringstream location; location << "Random location: "; // cur.push_back(y); - for (size_t pos = 0; pos < arm->getChain()->getNrOfJoints(); ++pos) - { + for (size_t pos = 0; pos < arm->getChain()->getNrOfJoints(); ++pos) { int val = rand() % 2; double diff = 0; - if (val == 1) + + if (val == 1) { diff = -0.1; - else + } + else { diff = 0.1; - // double val = (rand() % 70) / 100.0; + } + cur.push_back(preferred_state[pos] + diff); location << cur[pos] << " "; } @@ -286,10 +282,9 @@ private: return cur; } + // Use inverse kinematics to calculate joint positions to reach desired pose bool calc_joint_positions(const geometry_msgs::Pose &pose, const vector &start_state, vector &positions) { - // KDL::JntArray cur_configuration(arm->getChain()->getNrOfJoints()); // = arm_current_joint_states_; - // KDL::SetToZero(cur_configuration); KDL::Frame desired_end_effector_pose = pose_to_frame(pose); KDL::JntArray final_joint_configuration(arm->getChain()->getNrOfJoints()); @@ -306,16 +301,16 @@ private: // Convert data attribute (Eigen::VectorXd) of KDL::JntArray to double[] via data() function int nbr_joints = arm->getChain()->getNrOfJoints(); Eigen::VectorXd mat = final_joint_configuration.data; - // cout << mat << endl; positions.clear(); - // vector positions; std::stringstream pos_str; pos_str << "Calculated joint positions: "; + for (size_t idx = 0; idx < arm->getChain()->getNrOfJoints(); ++idx) { positions.push_back(mat[idx]); pos_str << mat[idx] << " "; } + pos_str << '\n'; ROS_INFO(pos_str.str().c_str()); @@ -323,6 +318,7 @@ private: return (error_code == 0); } + // Use forward kinematics to calculate end effector pose from current joint states KDL::Frame calc_end_effector_pose() { int num_joints = arm->getChain()->getNrOfJoints(); @@ -347,6 +343,7 @@ private: return end_effector_pose; } + // Convert from Pose to KDL::Frame KDL::Frame pose_to_frame(const geometry_msgs::Pose &pose) { double p_x = pose.position.x; @@ -363,6 +360,7 @@ private: return KDL::Frame(rot, pos); } + // Convert from KDL::Frame to Pose geometry_msgs::Pose frame_to_pose(const KDL::Frame &frame) { double p_x = frame.p.x(); @@ -386,6 +384,7 @@ private: return pose; } + // Send desired joint states to joint controller void send_joint_trajectory(const vector &target, arm_action_phase phase) { // Declare JointTrajectory message @@ -448,6 +447,7 @@ private: ros::Duration((num_points + 1) * time_per_step).sleep(); } + // Turn gripper on or off bool gripper_control(bool state) { osrf_gear::VacuumGripperControl srv; @@ -463,9 +463,10 @@ private: return success; } + // Check if the end effector has reached the target position (within threshold) bool have_reached_target(geometry_msgs::Pose cur, geometry_msgs::Pose target) { - // Tune threshold values + // Threshold values float pos_thresh = 0.15; // Meters float rot_thresh = 0.05; @@ -485,10 +486,10 @@ private: float qz_err = fabs(fabs(cur.orientation.z) - fabs(target.orientation.z)); float qw_err = fabs(fabs(cur.orientation.w) - fabs(target.orientation.w)); - ROS_INFO_STREAM("qx_err: " << qx_err << " (" << rot_thresh << ")"); - ROS_INFO_STREAM("qy_err: " << qy_err << " (" << rot_thresh << ")"); - ROS_INFO_STREAM("qz_err: " << qz_err << " (" << rot_thresh << ")"); - ROS_INFO_STREAM("qw_err: " << qw_err << " (" << rot_thresh << ")"); + // ROS_INFO_STREAM("qx_err: " << qx_err << " (" << rot_thresh << ")"); + // ROS_INFO_STREAM("qy_err: " << qy_err << " (" << rot_thresh << ")"); + // ROS_INFO_STREAM("qz_err: " << qz_err << " (" << rot_thresh << ")"); + // ROS_INFO_STREAM("qw_err: " << qw_err << " (" << rot_thresh << ")"); if (qx_err > rot_thresh) { @@ -516,7 +517,7 @@ private: int main(int argc, char **argv) { - cout << "Starting arm_controller_node" << endl; + ROS_INFO("Starting arm_controller_node"); // Last argument is the default name of the node. ros::init(argc, argv, "arm_controller_node"); @@ -525,19 +526,19 @@ int main(int argc, char **argv) ArmRepresentation arm; + // Set parameters double time_per_step = 3.0; // seconds int retry_attempts = 3; double item_attach_z_adjustment = 0.05; // meters ArmController ac(node, &arm, time_per_step, retry_attempts, item_attach_z_adjustment); + // Start asynchronous spinner to receive messages on subscribed topics ros::AsyncSpinner spinner(0); spinner.start(); - // Subscribe to arm destination and joint states channels + // Subscribe to ROS topics ros::Subscriber arm_destination_sub = node.subscribe(ARM_DESTINATION_CHANNEL, 1, &ArmController::arm_destination_callback, &ac); - ros::Subscriber gripper_state_sub = node.subscribe(GRIPPER_STATE_CHANNEL, 1, &ArmController::gripper_state_callback, &ac); - ros::Subscriber joint_state_sub = node.subscribe(ARM_JOINT_STATES_CHANNEL, 1, &ArmController::joint_state_callback, &ac); ROS_INFO("Setup complete"); diff --git a/src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp b/src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp index 5fa6e2e..c11b7b9 100644 --- a/src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp @@ -1,14 +1,14 @@ -// arm_controller_node.cpp +// arm_representation.cpp #include "arm_representation.h" #include -// using namespace std; -using std::string; -using std::vector; +using namespace std; +// Constructor ArmRepresentation::ArmRepresentation(const KDL::Frame &base_pose) { + // Parameters for coordinate transformations of robot links and joints const double base_len = 0.2; const double shoulder_height = 0.1273; const double upper_arm_length = 0.612; @@ -19,10 +19,7 @@ ArmRepresentation::ArmRepresentation(const KDL::Frame &base_pose) const double wrist_2_length = 0.1157; const double wrist_3_length = 0.0922; - // KDL::Vector pos; //(0, 0, base_len/2); - // KDL::Rotation rot; - - // The joints might be off by one + // Define chain of links and joints using KDL KDL::Segment linear_arm_actuator("linear_arm_actuator_joint", KDL::Joint(KDL::Joint::JointType::None), base_pose); chain.addSegment(linear_arm_actuator); @@ -74,23 +71,9 @@ ArmRepresentation::ArmRepresentation(const KDL::Frame &base_pose) KDL::Segment ee_link("ee_link", KDL::Joint("wrist_3_joint", KDL::Joint::JointType::RotY), //KDL::Joint(KDL::Joint::JointType::None), KDL::Frame(rot_ee, pos_ee)); chain.addSegment(ee_link); - - // arm.addSegment(base_link); - // arm.addSegment(shoulder_link); - // arm.addSegment(upper_arm_link); - // arm.addSegment(forearm_link); - // arm.addSegment(wrist_1_link); - // arm.addSegment(wrist_2_link); - // arm.addSegment(wrist_3_link); - // arm.addSegment(ee_link); - - // fk_solver = KDL::ChainFkSolverPos_recursive(chain); - // ik_solver = KDL::ChainIkSolverPos_LMA(chain); - - // fk_solver.reset(new KDL::ChainFkSolverPos_recursive(arm)); - // ik_solver.reset(new KDL::ChainIkSolverPos_LMA(arm)); } +// Use KDL to calculate position of end effector from joint states int ArmRepresentation::calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose, int joint_nbr) { KDL::ChainFkSolverPos_recursive fk_solver(chain); @@ -99,6 +82,7 @@ int ArmRepresentation::calculateForwardKinematics(const KDL::JntArray &joint_pos return status; } +// Use KDL to calculate joint states from position of end effector int ArmRepresentation::calculateInverseKinematics(const vector &cur_configuration, const KDL::Frame &desired_end_effector_pose, KDL::JntArray &final_joint_configuration) @@ -119,6 +103,7 @@ int ArmRepresentation::calculateInverseKinematics(const vector &cur_conf return status; } +// Get vector of joint names vector ArmRepresentation::get_joint_names() { vector joint_names; @@ -132,7 +117,8 @@ vector ArmRepresentation::get_joint_names() return joint_names; } +// Return reference to object KDL::Chain *ArmRepresentation::getChain() { return &chain; -} \ No newline at end of file +} diff --git a/src/rrrobot_ws/src/rrrobot/src/cv_model.py b/src/rrrobot_ws/src/rrrobot/src/cv_model.py index a666b34..dfcf096 100644 --- a/src/rrrobot_ws/src/rrrobot/src/cv_model.py +++ b/src/rrrobot_ws/src/rrrobot/src/cv_model.py @@ -1,4 +1,6 @@ #!/usr/bin/env python +# cv_model.py + import rospy from std_msgs.msg import String diff --git a/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp b/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp index f735cae..e78ce26 100644 --- a/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp @@ -252,21 +252,7 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg) // publish pose pub.publish(pose); - - // compute normals } - - //std::cerr<< "segmented: " << (int)point_cloud_segmented->size() << "\n"; - - // Convert to ROS data type - //point_cloud_segmented->header.frame_id = point_cloudPtr->header.frame_id; - //if(point_cloud_segmented->size()) pcl::toPCLPointCloud2(*point_cloud_segmented, cloud_filtered); - //else pcl::toPCLPointCloud2(*point_cloudPtr, cloud_filtered); - //sensor_msgs::PointCloud2 output; - //pcl_conversions::fromPCL(cloud_filtered, output); - - // Publish the data - //pub.publish (output); } int main(int argc, char **argv) @@ -283,4 +269,4 @@ int main(int argc, char **argv) pub = node.advertise(DESIRED_GRASP_POSE_CHANNEL, 1); ros::spin(); -} \ No newline at end of file +} diff --git a/src/rrrobot_ws/src/rrrobot/src/image_display.py b/src/rrrobot_ws/src/rrrobot/src/image_display.py index 622b5d3..e417400 100644 --- a/src/rrrobot_ws/src/rrrobot/src/image_display.py +++ b/src/rrrobot_ws/src/rrrobot/src/image_display.py @@ -1,4 +1,6 @@ #!/usr/bin/env python +# image_display.py + import rospy from std_msgs.msg import String import matplotlib.pyplot as plt @@ -18,4 +20,4 @@ def listener(): rospy.spin() if __name__ == '__main__': - listener() \ No newline at end of file + listener() diff --git a/src/rrrobot_ws/src/rrrobot/src/model_insertion_plugin.cpp b/src/rrrobot_ws/src/rrrobot/src/model_insertion_plugin.cpp index a863b4f..2a695ba 100644 --- a/src/rrrobot_ws/src/rrrobot/src/model_insertion_plugin.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/model_insertion_plugin.cpp @@ -1,3 +1,5 @@ +// model_insertion_plugin.cpp + #include #include "gazebo/physics/physics.hh" #include "gazebo/common/common.hh" @@ -71,4 +73,4 @@ private: // Register this plugin with the simulator GZ_REGISTER_WORLD_PLUGIN(ModelInsertion) -} // namespace gazebo \ No newline at end of file +} // namespace gazebo diff --git a/src/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp b/src/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp index fd3c18b..88aac11 100644 --- a/src/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp @@ -1,3 +1,5 @@ +// object_spawner_node.cpp + #include #include #include @@ -13,8 +15,7 @@ #include #include -using std::cout; -using std::endl; +using namespace std; class ObjectSpawner { @@ -123,4 +124,4 @@ int main(int argc, char **argv) ObjectSpawner spawner; ros::spin(); -} \ No newline at end of file +} diff --git a/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp b/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp index b9207ab..39990e6 100644 --- a/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp @@ -1,16 +1,4 @@ -// Copyright 2016 Open Source Robotics Foundation, 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. +// rrrobot_node.cpp #include #include @@ -38,8 +26,7 @@ #include "topic_names.h" #include "rrrobot/arm_command.h" -using std::cout; -using std::endl; +using namespace std; class Position { @@ -78,8 +65,7 @@ public: desired_drop_pose.position.x = drop_point.x; desired_drop_pose.position.y = drop_point.y; desired_drop_pose.position.z = drop_point.z; - // TODO: set the desired orientation of the end effector to point straight down - + // update state if (current_robot_state & RobotState::WAITING_FOR_GRAB_LOCATION) { @@ -93,7 +79,7 @@ public: publish_arm_command(); } - print_state(); + // print_state(); } void gripper_state_callback(const osrf_gear::VacuumGripperState &state) @@ -117,7 +103,7 @@ public: // store current state gripper_state = state; - print_state(); + // print_state(); } void grasp_pose_callback(const geometry_msgs::Pose &grasp_pose) @@ -126,7 +112,7 @@ public: set_conveyor(0); desired_grasp_pose = grasp_pose; - // TODO: Tune z offset so end effector doesn't hit object + // Add z offset so end effector doesn't hit object desired_grasp_pose.position.z += 0.01; if (current_robot_state & RobotState::WAITING_FOR_CLASSIFICATION) @@ -140,7 +126,7 @@ public: publish_arm_command(); } - print_state(); + // print_state(); } private: @@ -168,6 +154,7 @@ private: Position trash_bin = Position(-0.3, 0.383, 1); Position recycle_bin = Position(-0.3, 1.15, 1); + // Determine item destination bin based on classification Position destination(const std::string &type) const { Position pos; @@ -184,6 +171,7 @@ private: return pos; } + // Publish message including grab and drop off location for arm controller void publish_arm_command() { rrrobot::arm_command cmd; @@ -194,6 +182,7 @@ private: ros::spinOnce(); } + // Set conveyor power (0 or 50-100) void set_conveyor(int power) { if (power != 0 && (power < 50 || power > 100)) @@ -206,6 +195,7 @@ private: conveyor_pub.call(cmd); } + // Print current state for debugging void print_state() { if (current_robot_state & RobotState::WAITING_FOR_CLASSIFICATION) diff --git a/src/rrrobot_ws/src/rrrobot/test/test_arm.cpp b/src/rrrobot_ws/src/rrrobot/test/test_arm.cpp index 28400b5..02f0425 100644 --- a/src/rrrobot_ws/src/rrrobot/test/test_arm.cpp +++ b/src/rrrobot_ws/src/rrrobot/test/test_arm.cpp @@ -1,4 +1,4 @@ -// arm_controller_node.cpp +// test_arm.cpp #include #include @@ -106,4 +106,4 @@ int main(int argc, char **argv) cout << "x,y,z: " << end_effector_pose.p.x() << ", " << end_effector_pose.p.y() << ", " << end_effector_pose.p.z() << endl; cout << end_effector_pose.M << endl; } -} \ No newline at end of file +} diff --git a/src/rrrobot_ws/src/rrrobot/test/test_insert_object.cpp b/src/rrrobot_ws/src/rrrobot/test/test_insert_object.cpp index 559ce14..598d095 100644 --- a/src/rrrobot_ws/src/rrrobot/test/test_insert_object.cpp +++ b/src/rrrobot_ws/src/rrrobot/test/test_insert_object.cpp @@ -1,3 +1,5 @@ +// test_insert_object.cpp + #include "rrrobot/model_insertion.h" #include "ros/ros.h" @@ -36,4 +38,4 @@ int main(int argc, char **argv) pub.publish(msg); ros::spinOnce(); } -} \ No newline at end of file +}