diff --git a/docs/gear.md b/docs/gear.md index 88700a1..ace8587 100644 --- a/docs/gear.md +++ b/docs/gear.md @@ -86,18 +86,38 @@ rostopic pub /ariac/arm1/arm/command trajectory_msgs/JointTrajectory "{joint_ ## Running Full Simulation -### ARIAC Environment +### Terminal 1: ARIAC Environment - `cd /app/rrrobot_ws/src/rrrobot/scripts` -- `./rrrobot_run.sh` +- `./rrrobot_run_no_build.sh` -### Build RRRobot Package +### Terminal 2: Build RRRobot Package & Arm Controller Node - `cd /app/rrrobot_ws` - `catkin_make clean` - `catkin_make` - `source devel/setup.bash` - -### Arm Controller Node - - `rosrun rrrobot arm_controller_node` + +### Terminal 3: CV Model + +- `cd /app/rrrobot_ws/src/rrrobot/src/` +- `python3 cv_model.py` + +### Terminal 4: Depth Camera Node (for getting pick up location) + +- `cd /app/rrrobot_ws/` +- `source devel/setup.bash` +- `rosrun rrrobot depth_camera_node` + +### Terminal 5: Run the main rrrobot node + +- `cd /app/rrrobot_ws/` +- `source devel/setup.bash` +- `rosrun rrrobot rrrobot_node` + +### Terminal 6: Run the node to spawn random objects onto the conveyor belt + +- `cd /app/rrrobot_ws/` +- `source devel/setup.bash` +- `rosrun rrrobot object_spawner_node` diff --git a/src/rrrobot_ws/src/rrrobot/include/arm_representation.h b/src/rrrobot_ws/src/rrrobot/include/arm_representation.h index 6dc4a37..98dbdb4 100644 --- a/src/rrrobot_ws/src/rrrobot/include/arm_representation.h +++ b/src/rrrobot_ws/src/rrrobot/include/arm_representation.h @@ -43,7 +43,7 @@ public: int calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose, int joint_nbr = -1); - int calculateInverseKinematics(const KDL::JntArray &cur_configuration, + int calculateInverseKinematics(const std::vector &cur_configuration, const KDL::Frame &desired_end_effector_pose, KDL::JntArray &final_joint_configuration); diff --git a/src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test.sh b/src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test.sh index 9560048..190186d 100755 --- a/src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test.sh +++ b/src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test.sh @@ -1,11 +1,12 @@ #!/bin/bash -#rostopic pub /target_pose geometry_msgs/Pose '{position: {x: 0.875, y: 0.75, z: 1.5}, orientation: {x: 0, y: 0, z: 0, w: 0}}' +source /app/rrrobot_ws/devel/setup.bash + rostopic pub /arm_controller/destination rrrobot/arm_command "grab_location: position: - x: 1.2 - y: -1 - z: 1.5 + x: 1.22003614902 + y: 1.22740316391 + z: 0.97 orientation: x: 0.0 y: 0.707 @@ -14,11 +15,11 @@ rostopic pub /arm_controller/destination rrrobot/arm_command "grab_location: drop_location: position: x: -0.3 - y: 1.15 - z: 1.5 + y: 0.383 + z: 1.0 orientation: x: 0.0 - y: 0.0 + y: 0.707 z: 0.0 - w: 0.0" \ + w: 0.707" \ -1 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 e3b7353..7ece3b9 100644 --- a/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp @@ -48,10 +48,12 @@ public: gripper_ = node.serviceClient(GRIPPER_CONTROL_CHANNEL); + arm_current_joint_states_.resize(arm->getChain()->getNrOfJoints(), 0.0); + // 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_); + // arm_current_joint_states_ = KDL::JntArray(arm->getChain()->getNrOfJoints()); + // KDL::SetToZero(arm_current_joint_states_); } void joint_state_callback(const sensor_msgs::JointState &joint_state_msg) @@ -64,22 +66,9 @@ public: vector cur_joint_names = arm->get_joint_names(); vector position = joint_state_msg.position; - // Print joint name vectors for debugging - // ROS_INFO_STREAM("msg_joint_names: {"); - // for (int i = 0; i < nbr_joints; ++i) - // { - // ROS_INFO_STREAM(msg_joint_names[i] << ", "); - // } - // ROS_INFO_STREAM("}\ncur_joint_names: {"); - // for (int j = 0; j < cur_joint_names.size(); ++j) - // { - // ROS_INFO_STREAM(cur_joint_names[j] << ", "); - // } - // ROS_INFO_STREAM("}"); - for (int i = 0; i < nbr_joints; ++i) { - arm_current_joint_states_(i) = 0.0; + arm_current_joint_states_[i] = 0.0; } for (size_t i = 0; i < position.size(); ++i) @@ -88,7 +77,7 @@ public: { if (msg_joint_names[i].compare(cur_joint_names[j]) == 0) { - arm_current_joint_states_(j) = position[i]; + arm_current_joint_states_[j] = position[i]; } } } @@ -122,7 +111,7 @@ public: // Move arm to pickup item from conveyor belt attempts = 1; - positions = calc_joint_positions(target_pose.grab_location); + positions = calc_joint_positions(target_pose.grab_location, above_conveyor); send_joint_trajectory(positions, arm_action_phase::bin_to_belt); // Check if target has been reached @@ -151,7 +140,7 @@ public: // 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); + positions = calc_joint_positions(target_location, arm_current_joint_states_); send_joint_trajectory(positions, arm_action_phase::item_pickup_adjustment); attempts++; } @@ -160,7 +149,7 @@ public: // Move item to desired position attempts = 1; - positions = calc_joint_positions(target_pose.drop_location); + positions = calc_joint_positions(target_pose.drop_location, above_bins); send_joint_trajectory(positions, arm_action_phase::belt_to_bin); // Check if target has been reached @@ -198,7 +187,7 @@ public: ROS_ERROR("STEP 6: Item not detaching.. ¯\\_(ツ)_/¯"); } - ros::Duration(0.1).sleep(); + ros::Duration(1).sleep(); attempts++; } @@ -206,11 +195,12 @@ public: } private: + // Private variables bool gripper_enabled_; bool item_attached_; ros::Publisher arm_joint_trajectory_publisher_; ros::ServiceClient gripper_; - KDL::JntArray arm_current_joint_states_; + vector arm_current_joint_states_; ArmRepresentation *arm; enum arm_action_phase { belt_to_bin, @@ -220,8 +210,11 @@ private: double time_per_step; int retry_attempts; double item_attach_z_adjustment; + // 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}; - vector calc_joint_positions(const geometry_msgs::Pose &pose) + vector calc_joint_positions(const geometry_msgs::Pose &pose, const vector &start_state) { // KDL::JntArray cur_configuration(arm->getChain()->getNrOfJoints()); // = arm_current_joint_states_; // KDL::SetToZero(cur_configuration); @@ -230,7 +223,7 @@ private: // ROS_INFO("cur_configuration (%i x %i)", cur_configuration.rows(), cur_configuration.columns()); - int error_code = arm->calculateInverseKinematics(arm_current_joint_states_, desired_end_effector_pose, final_joint_configuration); + int error_code = arm->calculateInverseKinematics(start_state, desired_end_effector_pose, final_joint_configuration); // Check status of IK and print error message if there is a failure if (error_code != 0) @@ -253,13 +246,16 @@ private: KDL::Frame calc_end_effector_pose() { + int num_joints = arm->getChain()->getNrOfJoints(); + KDL::Frame end_effector_pose; - // KDL::JntArray joint_positions = arm_current_joint_states_; - KDL::JntArray joint_positions(arm->getChain()->getNrOfJoints()); - KDL::SetToZero(joint_positions); + KDL::JntArray joint_positions(num_joints); + + // Copy current joint states values into JntArray + for (int i = 0; i < num_joints; ++i) { + joint_positions(i) = arm_current_joint_states_[i]; + } - // ROS_INFO("joint_positions (%i x %i)", joint_positions.rows(), joint_positions.columns()); - // ROS_INFO("arm chain # of joints: %d", arm->getChain()->getNrOfJoints()); int error_code = arm->calculateForwardKinematics(joint_positions, end_effector_pose); // Check status of FK and do something if there is a failure @@ -315,10 +311,6 @@ private: // 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(); @@ -363,7 +355,7 @@ private: arm_joint_trajectory_publisher_.publish(msg); - // Wait to reach target + // TODO: Wait to reach target ros::Duration((num_points+1)*time_per_step).sleep(); } @@ -384,13 +376,15 @@ private: bool have_reached_target(geometry_msgs::Pose cur, geometry_msgs::Pose target) { // TODO: Tune threshold values - float pos_thresh = 0.1; // Meters + float pos_thresh = 0.15; // Meters float rot_thresh = 0.0875; // Radians float pos_err = fabs(cur.position.x - target.position.x) + fabs(cur.position.y - target.position.y) + fabs(cur.position.z - target.position.z); + ROS_INFO_STREAM("pos_err: " << pos_err << " (" << pos_thresh << ")"); + if (pos_err > pos_thresh) { return false; @@ -403,6 +397,15 @@ private: float ry_err = fabs(fmod(rpy1[1] - rpy2[1] + M_PI, M_PI)); float rz_err = fabs(fmod(rpy1[2] - rpy2[2] + M_PI, M_PI)); + // Errors now between 0 and pi + rx_err = min(rx_err, fabs(rx_err - (float) M_PI)); + ry_err = min(ry_err, fabs(ry_err - (float) M_PI)); + rz_err = min(rz_err, fabs(rz_err - (float) M_PI)); + + ROS_INFO_STREAM("rx_err: " << rx_err << " (" << rot_thresh << ")"); + ROS_INFO_STREAM("ry_err: " << ry_err << " (" << rot_thresh << ")"); + ROS_INFO_STREAM("rz_err: " << rz_err << " (" << rot_thresh << ")"); + if (rx_err > rot_thresh) { return false; @@ -449,9 +452,9 @@ int main(int argc, char **argv) // Subscribe to arm destination and joint states channels 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, 10, &ArmController::gripper_state_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, 10, &ArmController::joint_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 7caf630..5fa6e2e 100644 --- a/src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp @@ -99,12 +99,22 @@ int ArmRepresentation::calculateForwardKinematics(const KDL::JntArray &joint_pos return status; } -int ArmRepresentation::calculateInverseKinematics(const KDL::JntArray &cur_configuration, +int ArmRepresentation::calculateInverseKinematics(const vector &cur_configuration, const KDL::Frame &desired_end_effector_pose, KDL::JntArray &final_joint_configuration) { KDL::ChainIkSolverPos_LMA ik_solver(chain, 1e-3, 2000, 1e-8); - int status = ik_solver.CartToJnt(cur_configuration, desired_end_effector_pose, final_joint_configuration); + + // Convert cur_configuration vector to JntArray + int num_joints = chain.getNrOfJoints(); + KDL::JntArray pos(num_joints); + KDL::SetToZero(pos); + + for (int i = 0; i < num_joints; ++i) { + pos(i) = cur_configuration[i]; + } + + int status = ik_solver.CartToJnt(pos, desired_end_effector_pose, final_joint_configuration); return status; } diff --git a/src/rrrobot_ws/src/rrrobot/src/cv_model.py b/src/rrrobot_ws/src/rrrobot/src/cv_model.py index b02b735..e9fd9ef 100644 --- a/src/rrrobot_ws/src/rrrobot/src/cv_model.py +++ b/src/rrrobot_ws/src/rrrobot/src/cv_model.py @@ -38,7 +38,7 @@ def call_back(filename): # query model predicted_logits = model(input_tensor).squeeze() # print(predicted_logits.shape) - predicted_label = torch.argmax(predicted_logits).numpy().tolist() + predicted_label = torch.argmax(predicted_logits).detach().numpy().tolist() type_dict = {0: 'cardboard', 1: 'glass' , 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 812cd89..b3ecb5d 100644 --- a/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp @@ -218,7 +218,7 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg) geometry_msgs::Point p; geometry_msgs::Quaternion q; - // pick up from top + // pick up from top (-z direction) if (area_top > .002) { std::cout << "picking up object from top" << std::endl; @@ -228,23 +228,23 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg) // rpy = 0 0 0 q.x = 0; - q.y = 0; + q.y = 0.707; q.z = 0; - q.w = 1; + q.w = 0.707; } else { - // pick up from front + // pick up from front (+x direction) std::cout << "picking up object from front" << std::endl; p.x = xmin; p.y = (ymin + ymax) / 2; p.z = (zmax + zmin) / 2; // rpy = -pi/2 0 pi/2 - q.x = -0.63; - q.y = 0; - q.z = 0.63; - q.w = 0.44; + q.x = 0.707; + q.y = 0.0; + q.z = 0.0; + q.w = 0.707; } pose.position = p; diff --git a/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp b/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp index da7bad4..a657182 100644 --- a/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp @@ -118,6 +118,8 @@ public: set_conveyor(0); desired_grasp_pose = grasp_pose; + // TODO: Tune z offset so end effector doesn't hit object + desired_grasp_pose.position.z += 0.01; 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 74554bc..bae84d4 100644 --- a/src/rrrobot_ws/src/rrrobot/test/test_arm.cpp +++ b/src/rrrobot_ws/src/rrrobot/test/test_arm.cpp @@ -36,11 +36,7 @@ #include "arm_representation.h" -// using namespace std; -using std::cin; -using std::cout; -using std::endl; -using std::string; +using namespace std; // class ArmRepresentation // { @@ -190,7 +186,7 @@ int main(int argc, char **argv) f << end_effector_pose.p.x() << "," << end_effector_pose.p.y() << "," << end_effector_pose.p.z() << '\n'; - error_code = arm.calculateInverseKinematics(pos, end_effector_pose, pos); + error_code = arm.calculateInverseKinematics(vector(arm.getChain()->getNrOfJoints(), 0.0), end_effector_pose, pos); if (error_code != 0) {