diff --git a/piper_cpp/include/piper_cpp/fk.h b/piper_cpp/include/piper_cpp/fk.h index 9b2e4fc..5318ed1 100644 --- a/piper_cpp/include/piper_cpp/fk.h +++ b/piper_cpp/include/piper_cpp/fk.h @@ -44,8 +44,9 @@ class PiperForwardKinematics } /// Compute forward kinematics for the supplied joint angles (radians). Returns the - /// cumulative pose of each link as `[x, y, z, roll, pitch, yaw]` (mm + radians) in the - /// base frame. The last entry is the end-effector pose. + /// cumulative pose of each link as `[x, y, z, roll, pitch, yaw]` in the base frame, with + /// positions in mm and angles in degrees (the convention of the manufacturer's SDK, whose + /// home pose is `init_pos`). The last entry is the end-effector pose. std::vector> calcFK(const JointVec& cur_j) const { std::array Ts; diff --git a/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h b/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h index 128d60c..2346e75 100644 --- a/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h +++ b/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h @@ -220,10 +220,11 @@ class PiperInterfaceV2 /// Parsed firmware-version string. Looks for the `"S-V"` marker in the streamed firmware /// data and returns the 8-byte tag that follows it, e.g. `"S-V1.7-4"`. /// - /// Returns an empty optional if the marker hasn't been received yet (e.g. you've called + /// Returns an empty optional until the whole tag has been received (e.g. you've called /// this within the first ~100 ms of `connectPort` and the bytes haven't arrived). If you /// need to re-query (after a firmware-update on the arm, say), call - /// `requestFirmwareVersion()` and read this again a moment later. + /// `requestFirmwareVersion()`, which discards the previous reply, and read this again a + /// moment later. std::optional getFirmwareVersion() const; /// Re-send the firmware-version query (CAN ID `0x4AF`). The reply trickles in over the next @@ -232,8 +233,8 @@ class PiperInterfaceV2 bool requestFirmwareVersion(); /// Compute forward kinematics from the latest measured joint angles. Returns the cumulative - /// pose (position + orientation in the base frame) at each link, as a vector of 6-element - /// arrays. The last entry is the end-effector pose. + /// pose (position in mm + orientation in degrees, in the base frame) at each link, as a + /// vector of 6-element arrays. The last entry is the end-effector pose. /// /// @note This is computed on demand from `getArmJointStates()`; if the joint feedback isn't /// valid yet, returns the FK of zero joints and prints a warning to stderr. diff --git a/piper_cpp/src/can_utils.cpp b/piper_cpp/src/can_utils.cpp index 26b35f1..93917e8 100644 --- a/piper_cpp/src/can_utils.cpp +++ b/piper_cpp/src/can_utils.cpp @@ -82,6 +82,18 @@ static bool renameInterface(const std::string& old_name, const std::string& new_ return ok; } +// A parking name for an adapter awaiting its final name: short enough for IFNAMSIZ with any +// prefix, not currently in use, and never one of the final names. +static std::string freeTemporaryName(const std::vector& targets) +{ + for (int n = 0;; ++n) + { + const std::string tmp = "cantmp" + std::to_string(n); + if (if_nametoindex(tmp.c_str()) == 0 && std::find(targets.begin(), targets.end(), tmp) == targets.end()) + return tmp; + } +} + std::vector findPorts() { return getCanInterfacesInternal(); } std::vector activePorts() @@ -113,25 +125,46 @@ void activate(const std::vector& ports, const std::string& prefix, int throw std::runtime_error("Timed out waiting for CAN devices"); } std::sort(list.begin(), list.end(), [](auto& a, auto& b) { return a.second < b.second; }); + + // The kernel only renames an interface that is down, and the requested names may already + // be taken by the wrong adapters (two adapters swapping names), so: stop everything, park + // each adapter that needs renaming under a temporary name, then give out the final names + // and bring the interfaces up. + std::vector targets; + for (size_t i = 0; i < list.size(); ++i) + targets.push_back(prefix + std::to_string(i)); + std::vector current; for (size_t i = 0; i < list.size(); ++i) { const std::string iface = list[i].first; - const std::string target = prefix + std::to_string(i); - // stop interface if (can_do_stop(iface.c_str()) < 0) throw std::runtime_error("Failed to stop interface " + iface); - // set bitrate - if (can_set_bitrate(iface.c_str(), static_cast(bitrate)) < 0) - throw std::runtime_error("Failed to set bitrate on " + iface); - // start interface - if (can_do_start(iface.c_str()) < 0) - throw std::runtime_error("Failed to start interface " + iface); - // rename if necessary - if (target != iface) + std::string name = iface; + if (iface != targets[i]) + { + const std::string tmp = freeTemporaryName(targets); + if (!renameInterface(iface, tmp)) + std::cerr << "[WARN] rename " << iface << "->" << tmp << " failed"; + else + name = tmp; + } + current.push_back(name); + } + for (size_t i = 0; i < list.size(); ++i) + { + std::string name = current[i]; + const std::string& target = targets[i]; + if (name != target) { - if (!renameInterface(iface, target)) - std::cerr << "[WARN] rename " << iface << "->" << target << " failed"; + if (!renameInterface(name, target)) + std::cerr << "[WARN] rename " << name << "->" << target << " failed"; + else + name = target; } + if (can_set_bitrate(name.c_str(), static_cast(bitrate)) < 0) + throw std::runtime_error("Failed to set bitrate on " + name); + if (can_do_start(name.c_str()) < 0) + throw std::runtime_error("Failed to start interface " + name); } } diff --git a/piper_cpp/src/interface/piper_interface_v2.cpp b/piper_cpp/src/interface/piper_interface_v2.cpp index b8862f1..2afe869 100644 --- a/piper_cpp/src/interface/piper_interface_v2.cpp +++ b/piper_cpp/src/interface/piper_interface_v2.cpp @@ -6,7 +6,7 @@ namespace piper_cpp { -static constexpr double MILLIDEG2RAD = 0.001 * 3.14 / 180; +static constexpr double MILLIDEG2RAD = 0.001 * M_PI / 180; // Implementation of the PiperInterfaceV2 methods PiperInterfaceV2::PiperInterfaceV2( @@ -439,8 +439,11 @@ void PiperInterfaceV2::parseCANFrame(const struct can_frame &frame, double times } case ArmMsgType::FirmwareRead: { + // The reply to one query is a few frames; anything beyond this is noise, not the tag. + static constexpr size_t kMaxFirmwareBytes = 256; std::lock_guard lock(firmware_data_mutex_); - firmware_data_.insert(firmware_data_.end(), msg.firmware_data.begin(), msg.firmware_data.end()); + if (firmware_data_.size() < kMaxFirmwareBytes) + firmware_data_.insert(firmware_data_.end(), msg.firmware_data.begin(), msg.firmware_data.end()); break; } case ArmMsgType::InstructionResponseConfig: @@ -940,6 +943,12 @@ bool PiperInterfaceV2::sendFirmwareQuery() std::vector data{0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; uint16_t can_id = 0x4AF; + { + // Each query gets its own reply buffer, so a re-query after a firmware update cannot + // keep reporting the old tag. + std::lock_guard lock(firmware_data_mutex_); + firmware_data_.clear(); + } if (!arm_can_->sendCanMessage(can_id, data)) { std::cerr << "Error: Failed to send firmware query CAN message.\n"; @@ -966,8 +975,10 @@ std::optional PiperInterfaceV2::getFirmwareVersion() const { if (std::memcmp(firmware_data_.data() + i, marker, marker_len) == 0) { - const size_t end = std::min(i + version_len, firmware_data_.size()); - return std::string(firmware_data_.begin() + i, firmware_data_.begin() + end); + // The tag spans frames; report it only once it is all here. + if (i + version_len > firmware_data_.size()) + return std::nullopt; + return std::string(firmware_data_.begin() + i, firmware_data_.begin() + i + version_len); } } return std::nullopt; diff --git a/piper_cpp/test/CMakeLists.txt b/piper_cpp/test/CMakeLists.txt index 129628e..b83087b 100644 --- a/piper_cpp/test/CMakeLists.txt +++ b/piper_cpp/test/CMakeLists.txt @@ -14,3 +14,4 @@ add_piper_test(std_can_interface_test) add_piper_test(piper_interface_v2_test) add_piper_test(piper_parser_v2_test) add_piper_test(timed_freq_state_test) +add_piper_test(fk_test) diff --git a/piper_cpp/test/fk_test.cpp b/piper_cpp/test/fk_test.cpp new file mode 100644 index 0000000..979a15f --- /dev/null +++ b/piper_cpp/test/fk_test.cpp @@ -0,0 +1,26 @@ +#include "piper_cpp/fk.h" + +#include + +using piper_cpp::PiperForwardKinematics; + +// The manufacturer's SDK quotes the home pose of the default (offset) DH set in mm and +// degrees; the FK must reproduce it at zero joint angles, which pins the output units. The +// non-offset set's quoted pose is rounded to the millimetre, so it is not asserted. +TEST(ForwardKinematics, ZeroJointsGiveTheHomePoseInMillimetresAndDegrees) +{ + PiperForwardKinematics fk; + const auto pose = fk.calcFK({0, 0, 0, 0, 0, 0}).back(); + for (int i = 0; i < 6; ++i) + EXPECT_NEAR(pose[i], fk.init_pos[i], 0.001) << "component " << i; +} + +TEST(ForwardKinematics, InputIsRadians) +{ + PiperForwardKinematics fk; + // A quarter turn of joint 1 swings the home position from +x onto +y. + const auto pose = fk.calcFK({PiperForwardKinematics::PI / 2, 0, 0, 0, 0, 0}).back(); + EXPECT_NEAR(pose[0], 0.0, 0.01); + EXPECT_NEAR(pose[1], fk.init_pos[0], 0.01); + EXPECT_NEAR(pose[2], fk.init_pos[2], 0.01); +} diff --git a/piper_cpp/test/piper_interface_v2_test.cpp b/piper_cpp/test/piper_interface_v2_test.cpp index d9d742b..e86bdea 100644 --- a/piper_cpp/test/piper_interface_v2_test.cpp +++ b/piper_cpp/test/piper_interface_v2_test.cpp @@ -7,6 +7,7 @@ #include #include +#include #include #include #include @@ -210,6 +211,7 @@ TEST(PiperInterfaceV2, ConcurrentJointCommandsDoNotInterleave) // Each controlJoints() call is three frames (0x155, 0x156, 0x157). Two callers with // different targets must still produce whole triples on the bus, never a mix. constexpr int kCallsPerThread = 500; + std::signal(SIGPIPE, SIG_IGN); // a sender writing into the shut-down peer must fail, not die auto bus = makeBus(); ASSERT_TRUE(bus.piper->connectPort(/*can_init=*/false, /*piper_init=*/false, /*start_threads=*/false)); @@ -222,18 +224,24 @@ TEST(PiperInterfaceV2, ConcurrentJointCommandsDoNotInterleave) std::thread b(sender, 2000); // Receive failures are recorded rather than asserted here so the senders are always - // joined; a quiet bus means they have finished, so the joins cannot hang. + // joined; shutting the peer down first turns any blocked send into a failure, so the joins + // cannot hang. One deadline covers the whole exchange, so a scheduling pause between two + // frames does not count as a quiet bus. constexpr size_t kExpectedFrames = 2 * kCallsPerThread * 3; + const auto deadline = steady_clock::now() + 10s; std::vector frames; frames.reserve(kExpectedFrames); bool received_all = true; while (frames.size() < kExpectedFrames) { + const auto left = std::chrono::duration_cast(deadline - steady_clock::now()); pollfd pfd{bus.peer, POLLIN, 0}; can_frame f{}; - if (poll(&pfd, 1, 2000) <= 0 || ::recv(bus.peer, &f, sizeof(f), 0) != static_cast(sizeof(f))) + if (left.count() <= 0 || poll(&pfd, 1, static_cast(left.count())) <= 0 || + ::recv(bus.peer, &f, sizeof(f), 0) != static_cast(sizeof(f))) { received_all = false; + ::shutdown(bus.peer, SHUT_RDWR); break; } frames.push_back(f); @@ -250,7 +258,32 @@ TEST(PiperInterfaceV2, ConcurrentJointCommandsDoNotInterleave) ASSERT_EQ(frames[i + 1].can_id, 0x156u); ASSERT_EQ(frames[i + 2].can_id, 0x157u); const auto target = firstJoint(frames[i]); + ASSERT_TRUE(target == 1000 || target == 2000) << "triple " << i / 3 << " carries neither caller's target"; ASSERT_EQ(firstJoint(frames[i + 1]), target) << "triple " << i / 3 << " mixes two callers"; ASSERT_EQ(firstJoint(frames[i + 2]), target) << "triple " << i / 3 << " mixes two callers"; } } + +TEST(PiperInterfaceV2, FirmwareVersionNeedsTheWholeTagAndRestartsPerQuery) +{ + auto bus = makeBus(); + ASSERT_TRUE(bus.piper->connectPort(/*can_init=*/false, /*piper_init=*/false, /*start_threads=*/true)); + const auto deadline = steady_clock::now() + 5s; + auto firmwareBytes = [&](size_t n) { return bus.piper->getFirmwareData().size() == n; }; + + ASSERT_TRUE(bus.piper->requestFirmwareVersion()); + sendFrame(bus.peer, frame(0x4AF, {'S', '-', 'V', '1'}), deadline); + ASSERT_TRUE(waitFor([&] { return firmwareBytes(4); })); + EXPECT_FALSE(bus.piper->getFirmwareVersion()) << "half a tag is not a version"; + + sendFrame(bus.peer, frame(0x4AF, {'.', '7', '-', '4'}), deadline); + ASSERT_TRUE(waitFor([&] { return firmwareBytes(8); })); + EXPECT_EQ(bus.piper->getFirmwareVersion(), "S-V1.7-4"); + + // A new query must not keep answering with the previous reply. + ASSERT_TRUE(bus.piper->requestFirmwareVersion()); + EXPECT_FALSE(bus.piper->getFirmwareVersion()); + sendFrame(bus.peer, frame(0x4AF, {'S', '-', 'V', '2', '.', '0', '-', '1'}), deadline); + ASSERT_TRUE(waitFor([&] { return firmwareBytes(8); })); + EXPECT_EQ(bus.piper->getFirmwareVersion(), "S-V2.0-1"); +} diff --git a/piper_cpp_moveit/config/ompl_planning.yaml b/piper_cpp_moveit/config/ompl_planning.yaml index 6def06b..103c97a 100644 --- a/piper_cpp_moveit/config/ompl_planning.yaml +++ b/piper_cpp_moveit/config/ompl_planning.yaml @@ -10,17 +10,23 @@ response_adapters: - default_planning_response_adapters/ValidateSolution - default_planning_response_adapters/DisplayMotionPath +# Planner definitions live at the top level; each group lists the ones it may use by name. +planner_configs: + RRTConnect: + type: geometric::RRTConnect + range: 0.0 + RRTstar: + type: geometric::RRTstar + range: 0.0 + goal_bias: 0.05 + PRMstar: + type: geometric::PRMstar + manipulator: planner_configs: - RRTConnect: - type: geometric::RRTConnect - range: 0.0 - RRTstar: - type: geometric::RRTstar - range: 0.0 - goal_bias: 0.05 - PRMstar: - type: geometric::PRMstar + - RRTConnect + - RRTstar + - PRMstar default_planner_config: RRTConnect projection_evaluator: joints(joint1,joint2) longest_valid_segment_fraction: 0.005