diff --git a/config/bin_config.json b/config/bin_config.json index 85cbf6e..622cc31 100644 --- a/config/bin_config.json +++ b/config/bin_config.json @@ -1,4 +1,4 @@ { "description": "bin locations are [x, y] in m, relative to the left robot's lower left corner", - "locations": [[0.185, 0.068], [0.185, 0.102], [0.185, 0.135], [0.248, 0.068], [0.248, 0.102], [0.248, 0.135]] + "locations": [[0.210, 0.068], [0.210, 0.131], [0.210, 0.194], [0.273, 0.068], [0.273, 0.131], [0.273, 0.194]] } diff --git a/src/main.cpp b/src/main.cpp index 2ea3fee..446e425 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,3 +1,4 @@ +#include "expected.hpp" #include "json.hpp" #include "mdspan.hpp" #include "serial/serial.h" @@ -7,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -30,16 +32,19 @@ bin_coordinate offset_to_coord(int offset, int width = 3) { return {row, col}; } +// Parameters that change... robot state +// First joint (shoulder) is in index 0 +using joint_angles = std::array; + struct position_t { double x, y; }; -// Parameters that change... robot state? -// First joint (shoulder) is in index 0 -using joint_angles = std::array; +using path_t = std::vector; // Parameters that don't change struct kinematics_t { + std::string description; // link lengths // link from shoulder to elbow is index 0 std::array link; @@ -51,6 +56,135 @@ struct kinematics_t { bool elbow_up; }; +// struct robot_arm { +// robot_arm(std::string port, uint32_t baudrate, +// serial::Timeout timeout = serial::Timeout::simpleTimeout(10000)) +// : serial_(std::move(port), baudrate, std::move(timeout)) {} +// +// bin_occupancy is_bin_occupied(int bin) { +// if (!serial_.isOpen()) { +// throw std::runtime_error("Error opening serial port"); +// } +// serial_.write(std::to_string(bin)); +// auto const result = std::stoi(serial_.readline()); +// if (result == 1) { +// return bin_occupancy::OCCUPIED; +// } +// return bin_occupancy::EMPTY; +// } +// +// serial::Serial serial_; +// }; + +double to_radians(double degrees) { return degrees * std::numbers::pi / 180.; } +double to_degrees(double radians) { return radians * 180. / std::numbers::pi; } + +std::string serialize([[maybe_unused]] std::array const& joint_angle) { return ""; } + +struct robot_arm { + robot_arm(std::string port, uint32_t baudrate, + serial::Timeout timeout = serial::Timeout::simpleTimeout(10000)) + : serial_(std::move(port), baudrate, std::move(timeout)) {} + + tl::expected operator()(std::vector const& path) const { + if (!serial_.isOpen()) { + return tl::make_unexpected("Error opening serial port"); + } + // Convert the angles to degrees for arduino, rounded + std::vector> path_d; + std::ranges::transform( + path, std::back_inserter(path_d), [](auto const& j) -> std::array { + return {static_cast(to_radians(j[0])), static_cast(to_radians(j[1]))}; + }); + // Parse angles into string, maybe + // shoulder, shoulder\n + // as in + // 176, 25\n + // but it wouldn't surprise me if a header byte is needed, in which case + // {176, 25} might be just as well + std::ranges::for_each( + path_d, [this](auto const& joint_angle) { serial_.write(serialize(joint_angle)); }); + try { + // Send read sensor command so there isn't ambiguity about return values from the serial + serial_.write("this will be dependent on the parsing format"); + auto const result = std::stoi(serial_.readline()); + if (result == 1) { + return bin_occupancy::OCCUPIED; + } + return bin_occupancy::EMPTY; + } catch (std::invalid_argument const& e) { + return tl::make_unexpected("Invalid argument from serial port"); + } catch (std::out_of_range const& e) { + return tl::make_unexpected("Out of range from serial port"); + } + } + + mutable serial::Serial serial_; +}; + +struct bounds_checked_layout { + template + struct mapping : stdex::layout_right::mapping { + using extents_type = Extents; + using size_type = typename extents_type::size_type; + using base_t = stdex::layout_right::mapping; + using base_t::base_t; + size_type operator()(auto... idxs) const { + [&](std::index_sequence) { + if (((idxs < 0 || idxs >= this->extents().extent(Is)) || ...)) { + throw std::out_of_range("Invalid bin index"); + } + } + (std::make_index_sequence{}); + return this->base_t::operator()(idxs...); + } + }; +}; + +// Bin checker for single arm sync should +// reference access(data_handle_type ptr, std::ptrdiff_t offset) const { +// auto const goal = ptr[offset]; +// // give goal to robot arm +// // return what the robot arm sensed +// + +// TODO: This should still be shown somehow to demonstrate that no stack/heap memory +// is actually needed +// struct bin_checker { +// using element_type = bin_state; +// using reference = std::future; +// using data_handle_type = robot_arm*; +// +// reference access(data_handle_type ptr, std::ptrdiff_t offset) const { +// // We know ptr will be valid asynchronously because we construct it on the +// // stack of main. In real code we might want to use a shared_ptr or +// // something +// return std::async([=] { +// return bin_state{ptr->is_bin_occupied(static_cast(offset)), +// offset_to_coord(static_cast(offset))}; +// }); +// } +// }; + +// Also need a view to get the coordinates of a bin from json + +// using bin_view = stdex::mdspan, +// // Our layout should do bounds-checking +// bounds_checked_layout, +// // Our accessor should tell the robot to +// // asynchronously access the bin +// bin_checker>; + +using bin_grid_t = std::mdspan, bounds_checked_layout>; + +bool near(double a, double b, double tolerance = 0.00001) { return std::abs(a - b) < tolerance; } + +bool same(joint_angles const& lhs, joint_angles const& rhs) { + return near(lhs[0], rhs[0]) && near(lhs[1], rhs[1]); +} + // Kinematics for left arm // Given two joint andles, what is the x, y position_t forward_kinematics(joint_angles const& joint, kinematics_t config) { @@ -64,6 +198,13 @@ position_t forward_kinematics(joint_angles const& joint, kinematics_t config) { config.origin.y}; } +path_t forward_kinematics(std::vector const& joints, kinematics_t const& model) { + path_t path; + std::ranges::transform(joints, std::back_inserter(path), + [&](auto const& j) { return forward_kinematics(j, model); }); + return path; +} + joint_angles bounded_range(joint_angles const& angles) { double const first = std::atan2(std::sin(angles[0]), std::cos(angles[0])); double const second = std::atan2(std::sin(angles[1]), std::cos(angles[1])); @@ -72,7 +213,7 @@ joint_angles bounded_range(joint_angles const& angles) { // Inverse Kinematics // Given an x, y, and elbow config what are the joint angles -joint_angles inverse_kinematics(position_t goal, kinematics_t config) { +joint_angles inverse_kinematics(position_t goal, kinematics_t const& config) { // Transform the goal back into the arm frame goal.x -= config.origin.x; goal.y -= config.origin.y; @@ -89,71 +230,216 @@ joint_angles inverse_kinematics(position_t goal, kinematics_t config) { return bounded_range({std::atan2(y, x) + shoulderish, -elbow}); }; -struct robot_arm { - robot_arm(std::string port, uint32_t baudrate, - serial::Timeout timeout = serial::Timeout::simpleTimeout(10000)) - : serial_(std::move(port), baudrate, std::move(timeout)) {} +std::vector inverse_kinematics(path_t const& path, kinematics_t const& model) { + std::vector angles; + std::ranges::transform(path, std::back_inserter(angles), + [&](auto const& p) { return inverse_kinematics(p, model); }); + return angles; +} - bin_occupancy is_bin_occupied(int bin) { - if (!serial_.isOpen()) { - throw std::runtime_error("Error opening serial port"); - } - serial_.write(std::to_string(bin)); - auto const result = std::stoi(serial_.readline()); - if (result == 1) { - return bin_occupancy::OCCUPIED; - } - return bin_occupancy::EMPTY; +path_t lerp(position_t const& start, position_t const& goal, double step = 0.1) { + path_t path{start}; + for (double t = step; t < 1.; t += step) { + path.emplace_back(std::lerp(start.x, goal.x, t), std::lerp(start.y, goal.y, t)); } + path.push_back(goal); + return path; +} - serial::Serial serial_; -}; +std::vector lerp(joint_angles const& start, joint_angles const& goal, + double step = 0.1) { + std::vector path{start}; + for (double t = step; t < 1.; t += step) { + path.push_back({std::lerp(start[0], goal[0], t), std::lerp(start[1], goal[1], t)}); + } + path.push_back(goal); + return path; +} -struct bounds_checked_layout { - template - struct mapping : stdex::layout_right::mapping { - using extents_type = Extents; - using size_type = typename extents_type::size_type; - using base_t = stdex::layout_right::mapping; - using base_t::base_t; - size_type operator()(auto... idxs) const { - [&](std::index_sequence) { - if (((idxs < 0 || idxs >= this->extents().extent(Is)) || ...)) { - throw std::out_of_range("Invalid bin index"); - } - } - (std::make_index_sequence{}); - return this->base_t::operator()(idxs...); - } +position_t generate_waypoint(position_t const& p, kinematics_t const& model) { + auto const offset = std::copysign(0.02, model.origin.x - p.x); + return {p.x + offset, p.y}; +} + +// function that takes a goal and arm and returns a plan +path_t plan_cartesian(path_t const& waypoints) { + if (waypoints.empty()) { + return {{0., 0.}}; // Convert to std::expected + } + if (waypoints.size() == 1) { + return {waypoints[0]}; + } + // Add the first point to the path so that with subsequent plans + // we can prevent endpoint duplication + path_t path = {waypoints[0]}; + + for (std::size_t i = 0u; i < waypoints.size() - 1; ++i) { + auto const start = waypoints[i]; + auto const goal = waypoints[i + 1]; + auto const line = lerp(start, goal); + // Do not add the first point as it is already in the path + path.insert(path.end(), ++line.begin(), line.end()); + } + return path; +} + +// function that takes a goal and arm and returns a plan +std::vector plan_joint(path_t const& waypoints, kinematics_t const& model) { + if (waypoints.empty()) { + return {{0., 0.}}; // Convert to std::expected + } + if (waypoints.size() == 1) { + return inverse_kinematics(waypoints, model); + } + // Add the first joint angle to the path so that with subsequent plans + // we can prevent endpoint duplication + std::vector path = {inverse_kinematics(waypoints[0], model)}; + for (std::size_t i = 0u; i < waypoints.size() - 1; ++i) { + auto const start = inverse_kinematics(waypoints[i], model); + auto const goal = inverse_kinematics(waypoints[i + 1], model); + auto const line = lerp(start, goal); + // Do not add the first point as it is already in the path + path.insert(path.end(), ++line.begin(), line.end()); + } + return path; + // // Linearly interpolate between start and waypoint + // auto const jstart = inverse_kinematics(start, arm); + // auto const jwaypoint = inverse_kinematics(waypoint, arm); + // auto const p = lerp(jstart, jwaypoint); + // path.insert(path.end(), p.begin(), p.end()); + // // Linearly interpolate between waypoint and goal + // auto const jgoal = inverse_kinematics(goal, arm); + // auto const q = lerp(jwaypoint, jgoal); + // // trim off the first pose as it is the waypoint + // path.insert(path.end(), ++q.begin(), q.end()); + // return path; +} + +// function that takes arms current goal and returns which arm should service the goal +kinematics_t which_arm_nearest(position_t const& goal, std::array const& goals, + std::array const& arms) { + // Pair up the distance metrics with the arm configurations + std::array, 2> const distances = { + {{std::hypot(goal.x - goals[0].x, goal.y - goals[0].y), arms[0]}, + {std::hypot(goal.x - goals[1].x, goal.y - goals[1].y), arms[1]}}}; + // Find the nearest arm to goal + auto const argmin = std::min_element( + distances.begin(), distances.end(), + [](auto const& lhs, auto const& rhs) { return std::get<0>(lhs) < std::get<0>(rhs); }); + // Return the arm configuration + return std::get<1>(*argmin); +} + +// function that takes a goal and returns which arm can service it, based on left/right partition +kinematics_t which_arm_parition(position_t const& goal, std::array const& arms) { + return which_arm_nearest(goal, {{arms[0].origin, arms[1].origin}}, arms); +} + +// function to generate goals available to each arm, taking into account the 2d nature of the bins +// and the arm being on left or right, not considering other arm goals +std::vector available_goals(bin_grid_t bins, kinematics_t arm) { + // Check if the arm is to the left or the right + // If it is on the left, then the indices are: + if (arm.origin.x < bins(0, 0).x) { + return {bins(0, 0), bins(0, 1), bins(0, 2), bins(1, 0), bins(1, 2)}; + } + return {bins(0, 0), bins(0, 2), bins(1, 0), bins(1, 1), bins(1, 2)}; +} + +// function that takes available goal list, other arm goal, active arm and returns goal subset +std::vector available_goals(bin_grid_t bins, kinematics_t arm, position_t other_goal) { + // if the goal is within a ball of the other_goal and not in front (relative to the arm + // direction) + auto const in_ball = [&](auto const& goal) { + auto const adjacent = std::hypot(bins(0, 0).x - bins(0, 1).x, bins(0, 0).y - bins(0, 1).y); + // offset was tuned to prevent any goals from being available when an arm is crossing into + // the middle of the other robot's side + auto const offset = 0.03; + return std::hypot(other_goal.x - goal.x, other_goal.y - goal.y) < (adjacent + offset); }; -}; -struct bin_checker { - using element_type = bin_state; - using reference = std::future; - using data_handle_type = robot_arm*; + auto const is_behind = [&](auto const& goal) { + return (arm.origin.x <= other_goal.x && other_goal.x <= goal.x) || + (arm.origin.x >= other_goal.x && other_goal.x >= goal.x); + }; + + // Get the normal goal set + auto goals = available_goals(bins, arm); + std::erase_if(goals, [&](auto const& goal) { return is_behind(goal) && in_ball(goal); }); + return goals; +} + +// TODO: robot should return to waypoint after reading state +// TODO: on start up, robots should go to home +// TODO: on shut down, robots should go to home +// TODO: on the synchronous ones, should the value be reported after the moves are done? +// TODO: in arduino, need a joint angle to pwm function +// +// goal arbiter, single arm +// - goal comes in, add to queue +// - if robot is not available, wait until it is done with its current goal +// - now that robot is available +// - generate path to goal +// - command path +// - send joint command +// - wait for arm to report success +// - read sensor +// - report value +// - generate path to home +// - generate path to waypoint +// - generate path to home +// - command path + +bool elbow_up; +struct arbiter_single { + arbiter_single() + : model_{"left arm", + {0.150, 0.160 - 0.0085}, // 8.5 mm is the offset of the light detector from the end + // of the forearm + {0.026, 0.078}, // shoulder origin + std::numbers::pi / 2., // rotated relative to the workspace + false}, // left arm should be elbow down from it's perspective + state_{0., 0.}, + home_{forward_kinematics(state_, model_)}, + arm_{"/dev/ttyACM0", 9600} {} - reference access(data_handle_type ptr, std::ptrdiff_t offset) const { - // We know ptr will be valid asynchronously because we construct it on the - // stack of main. In real code we might want to use a shared_ptr or - // something - return std::async([=] { - return bin_state{ptr->is_bin_occupied(static_cast(offset)), - offset_to_coord(static_cast(offset))}; - }); + tl::expected operator()(position_t const& goal) { + auto const near_goal = generate_waypoint(goal, model_); + auto const goto_goal = inverse_kinematics(plan_cartesian({home_, near_goal, goal}), model_); + auto const sensor_reading_maybe = arm_(goto_goal); + // Out current cartesian position should be at the goal + auto const goto_home = inverse_kinematics(plan_cartesian({goal, near_goal, home_}), model_); + arm_(goto_home); + return sensor_reading_maybe; } + + private: + kinematics_t model_; + joint_angles state_; + position_t home_; + robot_arm arm_; }; -// Also need a view to get the coordinates of a bin from json +// goal arbiter, dual arm partitioned +// - goal comes in +// - add goal to the queue of the correct robot +// - goal arbiter single arm -using bin_view = stdex::mdspan, - // Our layout should do bounds-checking - bounds_checked_layout, - // Our accessor should tell the robot to - // asynchronously access the bin - bin_checker>; +// goal arbiter, dual arm non-partitioned +// - goal comes in, add to set +// +// - when an arm becomes available +// - get the current goal of the other arm +// - get the list of available goals +// - check if any of the available goals are in the goal set +// - if there is more than one, choose one that is closest to current position +// - generate path to goal +// - command path +// - read sensor, report async? +// - get waypoint next to goal +// - generate path to waypoint +// - command path +// auto print_state = [](bin_state const& state) { std::cout << "Bin at (" << state.coordinate.x << ", " << state.coordinate.y << ") is "; @@ -168,38 +454,37 @@ auto print_state = [](bin_state const& state) { std::cout << "\n"; }; -#include -bool near(double a, double b, double tolerance = 0.00001) { return std::abs(a - b) < tolerance; } - -bool same(joint_angles const& lhs, joint_angles const& rhs) { - return near(lhs[0], rhs[0]) && near(lhs[1], rhs[1]); -} - int main() { std::ifstream bin_file{"src/spanny2/config/bin_config.json"}; json bin_config = json::parse(bin_file); std::cout << bin_config.dump(2) << std::endl; std::array bin_positions; - std::mdspan, bounds_checked_layout> bin_setter{ - bin_positions.data()}; - for (std::size_t ndx = 0; ndx < bin_setter.extent(0); ndx++) { - auto const position = bin_config["locations"][ndx]; - bin_setter(ndx) = position_t{position[0], position[1]}; + std::ranges::transform(bin_config["locations"].get>>(), + bin_positions.begin(), [](auto const& p) { + return position_t{p[0], p[1]}; + }); + + bin_grid_t bin_grid{bin_positions.data()}; + for (auto i = 0u; i < bin_grid.extent(0); ++i) { + for (auto j = 0u; j < bin_grid.extent(1); ++j) { + auto const p = bin_grid(i, j); + std::cout << "(" << i << ", " << j << ") = {" << p.x << ", " << p.y << "}\n"; + } } - std::mdspan, bounds_checked_layout> bin_grid{ - bin_positions.data()}; kinematics_t left_arm, right_arm; + left_arm.description = "left arm"; // 8.5 mm is the offset of the light detector from the end of the forearm left_arm.link = {0.150, 0.160 - 0.0085}; left_arm.origin = {0.026, 0.078}; left_arm.orientation = std::numbers::pi / 2.; left_arm.elbow_up = false; + right_arm.description = "right arm"; right_arm.link = left_arm.link; - // 0.381 is the offset for the right arm base - right_arm.origin = {0.026 + 0.381, 0.078}; + // 0.431 is the offset for the right arm base + right_arm.origin = {0.026 + 0.431, 0.078}; right_arm.orientation = std::numbers::pi / 2.; right_arm.elbow_up = true; @@ -216,24 +501,44 @@ int main() { std::cout << "left end_effector = {" << end_effector.x << ", " << end_effector.y << "}\n"; std::cout << "angles = {" << angles[0] << ", " << angles[1] << "}\n"; } - return 0; - - auto arm = robot_arm{"/dev/ttyACM0", 9600}; - auto bins = bin_view(&arm); - while (true) { - std::vector> futures; - for (auto i = 0u; i < bins.extent(0); ++i) { - for (auto j = 0u; j < bins.extent(1); ++j) { - futures.push_back(bins(i, j)); - } - } - for (auto const& future : futures) { - future.wait(); - } - for (auto& future : futures) { - print_state(future.get()); - } - std::cout << "====================" << std::endl; + auto const path = plan_cartesian({forward_kinematics({0, 0}, right_arm), + generate_waypoint(bin_grid(0, 0), right_arm), bin_grid(0, 0)}); + for (auto const& p : path) { + // auto const j = inverse_kinematics(p, right_arm); + // std::cout << j[0] << ", " << j[1] << "\n"; + std::cout << p.x << ", " << p.y << "\n"; } + + std::ranges::for_each(bin_positions, [&](auto const& goal) { + auto const arm = which_arm_parition(goal, {left_arm, right_arm}); + std::cout << arm.description << " {" << goal.x << ", " << goal.y << "}\n"; + }); + + std::ranges::for_each(bin_positions, [&](auto const other_goal) { + std::cout << left_arm.description << " other_goal = {" << other_goal.x << ", " << other_goal.y + << "}\n"; + // auto const goals = available_goals(bin_grid, left_arm); + auto const goals = available_goals(bin_grid, left_arm, other_goal); + std::ranges::for_each(goals, [](auto const& p) { std::cout << p.x << ", " << p.y << "\n"; }); + }); return 0; + + // auto arm = robot_arm{"/dev/ttyACM0", 9600}; + // auto bins = bin_view(&arm); + // while (true) { + // std::vector> futures; + // for (auto i = 0u; i < bins.extent(0); ++i) { + // for (auto j = 0u; j < bins.extent(1); ++j) { + // futures.push_back(bins(i, j)); + // } + // } + // for (auto const& future : futures) { + // future.wait(); + // } + // for (auto& future : futures) { + // print_state(future.get()); + // } + // std::cout << "====================" << std::endl; + // } + // return 0; }