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 190186d..1dc4c79 100755 --- a/src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test.sh +++ b/src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test.sh @@ -4,9 +4,9 @@ source /app/rrrobot_ws/devel/setup.bash rostopic pub /arm_controller/destination rrrobot/arm_command "grab_location: position: - x: 1.22003614902 - y: 1.22740316391 - z: 0.97 + x: 1.22 + y: 1.22 + z: 0.9725 orientation: x: 0.0 y: 0.707 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 aef7cb6..c5495b6 100644 --- a/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp @@ -355,7 +355,7 @@ private: arm_joint_trajectory_publisher_.publish(msg); - // TODO: Wait to reach target + // Wait to reach target ros::Duration((num_points+1)*time_per_step).sleep(); } @@ -375,9 +375,9 @@ private: bool have_reached_target(geometry_msgs::Pose cur, geometry_msgs::Pose target) { - // TODO: Tune threshold values + // Tune threshold values float pos_thresh = 0.15; // Meters - float rot_thresh = 0.025; + float rot_thresh = 0.05; float pos_err = fabs(cur.position.x - target.position.x) + fabs(cur.position.y - target.position.y) + @@ -436,7 +436,7 @@ int main(int argc, char **argv) double time_per_step = 3.0; // seconds int retry_attempts = 3; - double item_attach_z_adjustment = 0.025; // meters + double item_attach_z_adjustment = 0.05; // meters ArmController ac(node, &arm, time_per_step, retry_attempts, item_attach_z_adjustment); ros::AsyncSpinner spinner(0); 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 b3ecb5d..f735cae 100644 --- a/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp @@ -271,18 +271,16 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg) int main(int argc, char **argv) { - // TODO: Subscribe to depth camera topic // Last argument is the default name of the node. ros::init(argc, argv, "depth_camera_node"); ros::NodeHandle node; + // Subscribe to depth camera topic ros::Subscriber sub = node.subscribe("/ariac/depth_camera_1", 1, depth_camera_callback); + // Publish object's current location to topic for RRRobot node to listen to pub = node.advertise(DESIRED_GRASP_POSE_CHANNEL, 1); ros::spin(); - // TODO: When item is in view, work with point cloud to get location (in world frame) for arm to reach to pickup item - - // TODO: Publish object's current location to topic for RRRobot node to listen to } \ No newline at end of file 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 5eacaf2..0b3a6fb 100644 --- a/src/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp @@ -99,7 +99,7 @@ private: image_file_msg.data = models[rand_idx].image_file; cout << "Spawning " << msg.model_name << endl; - // TODO: Determine position to spawn items + // Determine position to spawn items // can place objects on conveyor belt just upstream from the arms // at roughly (0.2516105, 5.474367, 0.935669) // x range: 1.022548-1.413952 diff --git a/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp b/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp index a657182..ecc9e1c 100644 --- a/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp @@ -119,7 +119,7 @@ public: desired_grasp_pose = grasp_pose; // TODO: Tune z offset so end effector doesn't hit object - desired_grasp_pose.position.z += 0.01; + desired_grasp_pose.position.z -= 0.025; 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 03bb074..28400b5 100644 --- a/src/rrrobot_ws/src/rrrobot/test/test_arm.cpp +++ b/src/rrrobot_ws/src/rrrobot/test/test_arm.cpp @@ -38,124 +38,6 @@ using namespace std; -// class ArmRepresentation -// { -// public: -// ArmRepresentation(const KDL::Frame &base_pose = KDL::Frame(KDL::Vector(0.3, 0.92, 1))) -// { -// const double base_len = 0.2; -// const double shoulder_height = 0.1273; -// const double upper_arm_length = 0.612; -// const double forearm_length = 0.5723; -// const double shoulder_offset = 0.220941; -// const double elbow_offset = -0.1719; -// const double wrist_1_length = 0.163941 - elbow_offset - shoulder_offset; -// 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 -// KDL::Segment linear_arm_actuator("linear_arm_actuator_joint", -// KDL::Joint(KDL::Joint::JointType::None), base_pose); -// arm.addSegment(linear_arm_actuator); - -// const KDL::Vector pos_base(0, 0, base_len / 2); -// const KDL::Rotation rot_base(KDL::Rotation::RPY(0, 0, 0)); -// KDL::Segment base_link("base_link", KDL::Joint(KDL::Joint::JointType::TransY), -// KDL::Frame(rot_base, pos_base)); -// arm.addSegment(base_link); - -// const KDL::Vector pos_shoulder(0, 0, shoulder_height); -// const KDL::Rotation rot_shoulder(KDL::Rotation::RPY(0, 0, 0)); -// KDL::Segment shoulder_link("shoulder_link", KDL::Joint(KDL::Joint::JointType::RotZ), -// KDL::Frame(rot_shoulder, pos_shoulder)); -// arm.addSegment(shoulder_link); - -// const KDL::Vector pos_upper_arm(0, shoulder_offset, 0); -// const KDL::Rotation rot_upper_arm(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0)); -// KDL::Segment upper_arm_link("upper_arm_link", KDL::Joint(KDL::Joint::JointType::RotY), -// KDL::Frame(rot_upper_arm, pos_upper_arm)); -// arm.addSegment(upper_arm_link); - -// const KDL::Vector pos_forearm(0, elbow_offset, upper_arm_length); -// const KDL::Rotation rot_forearm(KDL::Rotation::RPY(0.0, 0.0, 0.0)); -// KDL::Segment forearm_link("forearm_link", KDL::Joint(KDL::Joint::JointType::RotY), -// KDL::Frame(rot_forearm, pos_forearm)); -// arm.addSegment(forearm_link); - -// const KDL::Vector pos_wrist_1(0, 0, forearm_length); -// const KDL::Rotation rot_wrist_1(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0)); -// KDL::Segment wrist_1_link("wrist_1_link", KDL::Joint(KDL::Joint::JointType::RotY), -// KDL::Frame(rot_wrist_1, pos_wrist_1)); -// arm.addSegment(wrist_1_link); - -// const KDL::Vector pos_wrist_2(0, wrist_1_length, 0); -// const KDL::Rotation rot_wrist_2(KDL::Rotation::RPY(0.0, 0.0, 0.0)); -// KDL::Segment wrist_2_link("wrist_2_link", KDL::Joint(KDL::Joint::JointType::RotZ), -// KDL::Frame(rot_wrist_2, pos_wrist_2)); -// arm.addSegment(wrist_2_link); - -// const KDL::Vector pos_wrist_3(0, 0, wrist_2_length); -// const KDL::Rotation rot_wrist_3(KDL::Rotation::RPY(0.0, 0.0, 0.0)); -// KDL::Segment wrist_3_link("wrist_3_link", KDL::Joint(KDL::Joint::JointType::RotY), -// KDL::Frame(rot_wrist_3, pos_wrist_3)); -// arm.addSegment(wrist_3_link); - -// const KDL::Vector pos_ee(0, wrist_3_length, 0.0); -// const KDL::Rotation rot_ee(KDL::Rotation::RPY(0.0, 0.0, M_PI / 2.0)); -// KDL::Segment ee_link("ee_link", KDL::Joint(KDL::Joint::JointType::None), -// KDL::Frame(rot_ee, pos_ee)); -// arm.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.reset(new KDL::ChainFkSolverPos_recursive(arm)); -// ik_solver.reset(new KDL::ChainIkSolverPos_LMA(arm)); -// } - -// int calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose) -// { -// return fk_solver->JntToCart(joint_positions, end_effector_pose); -// } - -// int calculateInverseKinematics(const KDL::JntArray &cur_configuration, -// const KDL::Frame &desired_end_effector_pose, -// KDL::JntArray &final_joint_configuration) -// { -// return ik_solver->CartToJnt(cur_configuration, desired_end_effector_pose, final_joint_configuration); -// } - -// string *get_joint_names() -// { -// // TODO: return ["linear_arm_actuator_joint", -// // "shoulder_pan_joint", -// // "shoulder_lift_joint", -// // "elbow_joint", -// // "wrist_1_joint", -// // "wrist_2_joint", -// // "wrist_3_joint"]; -// } - -// const KDL::Chain &getArm() const -// { -// return arm; -// } - -// private: -// KDL::Chain arm; - -// std::unique_ptr fk_solver; -// std::unique_ptr ik_solver; -// }; int main(int argc, char **argv) {