diff --git a/src/viam/sdk/components/arm.hpp b/src/viam/sdk/components/arm.hpp index 8a75df707..e0ff7bba9 100644 --- a/src/viam/sdk/components/arm.hpp +++ b/src/viam/sdk/components/arm.hpp @@ -5,10 +5,10 @@ #include +#include #include #include -#include #include #include @@ -61,6 +61,12 @@ class Arm : public Component, public Stoppable { using KinematicsData = boost::variant; + /// @brief Movement specifications for move_through_join_positions. + struct MoveOptions { + boost::optional max_vel_degs_per_sec; + boost::optional max_acc_degs_per_sec2; + }; + static KinematicsData from_proto(const viam::common::v1::GetKinematicsResponse& proto); /// @brief Get the current position of the end of the arm. @@ -103,6 +109,22 @@ class Arm : public Component, public Stoppable { virtual void move_to_joint_positions(const std::vector& positions, const ProtoStruct& extra) = 0; + /// @brief Move each joint on the arm through the positions specified in @param positions + /// @param options optional specifications to be obeyed during the motion. + /// TODO consider replacing vector vector with xtensor array, and also if it may be + /// possible to specify or constrain dimensionality of the array in advance. + inline void move_through_joint_positions(const std::vector>& positions, + const MoveOptions& options) { + return move_through_joint_positions(positions, options, {}); + } + + /// @brief Move each joint on the arm through the positions specified in @param positions + /// @param options optional specifications to be obeyed during the motion. + /// @param extra Any additional arguments to the method. + virtual void move_through_joint_positions(const std::vector>& positions, + const MoveOptions& options, + const ProtoStruct& extra) = 0; + /// @brief Reports if the arm is in motion. virtual bool is_moving() = 0; diff --git a/src/viam/sdk/components/private/arm_client.cpp b/src/viam/sdk/components/private/arm_client.cpp index 8ec6b8a6b..a3899036d 100644 --- a/src/viam/sdk/components/private/arm_client.cpp +++ b/src/viam/sdk/components/private/arm_client.cpp @@ -46,6 +46,31 @@ void ArmClient::move_to_joint_positions(const std::vector& positions, .invoke(); } +void ArmClient::move_through_joint_positions(const std::vector>& positions, + const Arm::MoveOptions& options, + const ProtoStruct& extra) { + return make_client_helper(this, *stub_, &StubType::MoveThroughJointPositions) + .with(extra, + [&](viam::component::arm::v1::MoveThroughJointPositionsRequest& request) { + if (options.max_vel_degs_per_sec) { + request.mutable_options()->set_max_vel_degs_per_sec( + *options.max_vel_degs_per_sec); + } + + if (options.max_acc_degs_per_sec2) { + request.mutable_options()->set_max_acc_degs_per_sec2( + *options.max_acc_degs_per_sec2); + } + + for (const auto& pos : positions) { + viam::component::arm::v1::JointPositions jpos; + jpos.mutable_values()->Add(pos.begin(), pos.end()); + request.mutable_positions()->Add(std::move(jpos)); + } + }) + .invoke(); +} + bool ArmClient::is_moving() { return make_client_helper(this, *stub_, &StubType::IsMoving).invoke([](auto& response) { return response.is_moving(); diff --git a/src/viam/sdk/components/private/arm_client.hpp b/src/viam/sdk/components/private/arm_client.hpp index 57737a08a..6608a1340 100644 --- a/src/viam/sdk/components/private/arm_client.hpp +++ b/src/viam/sdk/components/private/arm_client.hpp @@ -26,6 +26,9 @@ class ArmClient : public Arm { std::vector get_joint_positions(const ProtoStruct& extra) override; void move_to_joint_positions(const std::vector& positions, const ProtoStruct& extra) override; + void move_through_joint_positions(const std::vector>& positions, + const Arm::MoveOptions& options, + const ProtoStruct& extra) override; bool is_moving() override; void stop(const ProtoStruct& extra) override; ProtoStruct do_command(const ProtoStruct& command) override; @@ -38,6 +41,7 @@ class ArmClient : public Arm { using Arm::get_geometries; using Arm::get_joint_positions; using Arm::get_kinematics; + using Arm::move_through_joint_positions; using Arm::move_to_joint_positions; using Arm::move_to_position; using Arm::stop; diff --git a/src/viam/sdk/components/private/arm_server.cpp b/src/viam/sdk/components/private/arm_server.cpp index 93a903b67..fad2c0399 100644 --- a/src/viam/sdk/components/private/arm_server.cpp +++ b/src/viam/sdk/components/private/arm_server.cpp @@ -53,6 +53,32 @@ ::grpc::Status ArmServer::MoveToJointPositions( }); } +::grpc::Status ArmServer::MoveThroughJointPositions( + ::grpc::ServerContext*, + const ::viam::component::arm::v1::MoveThroughJointPositionsRequest* request, + ::viam::component::arm::v1::MoveThroughJointPositionsResponse*) noexcept { + return make_service_helper( + "ArmServer::MoveThroughJointPositions", this, request)([&](auto& helper, auto& arm) { + std::vector> positions; + + positions.reserve(request->positions_size()); + for (const auto& values : request->positions()) { + positions.emplace_back(values.values().begin(), values.values().end()); + } + + Arm::MoveOptions opts; + if (request->options().has_max_vel_degs_per_sec()) { + opts.max_vel_degs_per_sec = request->options().max_vel_degs_per_sec(); + } + + if (request->options().has_max_acc_degs_per_sec2()) { + opts.max_acc_degs_per_sec2 = request->options().max_acc_degs_per_sec2(); + } + + arm->move_through_joint_positions(positions, opts, helper.getExtra()); + }); +} + ::grpc::Status ArmServer::Stop(::grpc::ServerContext*, const ::viam::component::arm::v1::StopRequest* request, ::viam::component::arm::v1::StopResponse*) noexcept { diff --git a/src/viam/sdk/components/private/arm_server.hpp b/src/viam/sdk/components/private/arm_server.hpp index 5390e42f0..f19788b60 100644 --- a/src/viam/sdk/components/private/arm_server.hpp +++ b/src/viam/sdk/components/private/arm_server.hpp @@ -44,6 +44,11 @@ class ArmServer : public ResourceServer, public viam::component::arm::v1::ArmSer const ::viam::component::arm::v1::MoveToJointPositionsRequest* request, ::viam::component::arm::v1::MoveToJointPositionsResponse* response) noexcept override; + ::grpc::Status MoveThroughJointPositions( + ::grpc::ServerContext* context, + const ::viam::component::arm::v1::MoveThroughJointPositionsRequest* request, + ::viam::component::arm::v1::MoveThroughJointPositionsResponse* response) noexcept override; + ::grpc::Status Stop(::grpc::ServerContext* context, const ::viam::component::arm::v1::StopRequest* request, ::viam::component::arm::v1::StopResponse* response) noexcept override; diff --git a/src/viam/sdk/tests/mocks/mock_arm.cpp b/src/viam/sdk/tests/mocks/mock_arm.cpp index 06c54ed0f..2eb4c8fb0 100644 --- a/src/viam/sdk/tests/mocks/mock_arm.cpp +++ b/src/viam/sdk/tests/mocks/mock_arm.cpp @@ -1,5 +1,6 @@ #include +#include "mock_arm.hpp" #include namespace viam { @@ -31,6 +32,13 @@ void MockArm::move_to_joint_positions(const std::vector& positions, joint_positions = positions; } +void MockArm::move_through_joint_positions(const std::vector>& positions, + const Arm::MoveOptions& opts, + const sdk::ProtoStruct&) { + move_thru_positions = positions; + move_opts = opts; +} + void MockArm::stop(const sdk::ProtoStruct&) { peek_stop_called = true; } diff --git a/src/viam/sdk/tests/mocks/mock_arm.hpp b/src/viam/sdk/tests/mocks/mock_arm.hpp index cb0139e55..fdd7537eb 100644 --- a/src/viam/sdk/tests/mocks/mock_arm.hpp +++ b/src/viam/sdk/tests/mocks/mock_arm.hpp @@ -19,6 +19,11 @@ class MockArm : public sdk::Arm { std::vector get_joint_positions(const sdk::ProtoStruct&) override; void move_to_joint_positions(const std::vector& positions, const sdk::ProtoStruct&) override; + + void move_through_joint_positions(const std::vector>& positions, + const Arm::MoveOptions& opts, + const sdk::ProtoStruct&) override; + void stop(const sdk::ProtoStruct&) override; bool is_moving() override; sdk::ProtoStruct do_command(const sdk::ProtoStruct& command) override; @@ -27,6 +32,8 @@ class MockArm : public sdk::Arm { sdk::pose current_location; std::vector joint_positions; + std::vector> move_thru_positions; + sdk::Arm::MoveOptions move_opts; bool peek_stop_called; sdk::ProtoStruct peek_command; }; diff --git a/src/viam/sdk/tests/test_arm.cpp b/src/viam/sdk/tests/test_arm.cpp index 2a60488cf..933fe781b 100644 --- a/src/viam/sdk/tests/test_arm.cpp +++ b/src/viam/sdk/tests/test_arm.cpp @@ -1,14 +1,16 @@ #define BOOST_TEST_MODULE test module test_arm +#include +#include #include #include -#include #include #include BOOST_TEST_DONT_PRINT_LOG_VALUE(std::vector) BOOST_TEST_DONT_PRINT_LOG_VALUE(std::vector) +BOOST_TEST_DONT_PRINT_LOG_VALUE(std::vector>) BOOST_TEST_DONT_PRINT_LOG_VALUE(viam::sdk::Arm::KinematicsData) namespace viam { @@ -49,6 +51,17 @@ BOOST_AUTO_TEST_CASE(joint_positions) { }); } +BOOST_AUTO_TEST_CASE(thru_joint_positions) { + std::shared_ptr mock = MockArm::get_mock_arm(); + client_to_mock_pipeline(mock, [&](Arm& client) { + std::vector> positions{{1.0, 2.0}, {3.0}}; + client.move_through_joint_positions(positions, {1.0, 2.0}, {}); + BOOST_CHECK_EQUAL(mock->move_thru_positions, positions); + BOOST_CHECK_EQUAL(mock->move_opts.max_vel_degs_per_sec, 1.0); + BOOST_CHECK_EQUAL(mock->move_opts.max_acc_degs_per_sec2, 2.0); + }); +} + BOOST_AUTO_TEST_CASE(test_stop) { std::shared_ptr mock = MockArm::get_mock_arm(); client_to_mock_pipeline(mock, [&](Arm& client) {