Add MoveThroughJointPositions to Arm component (#324)

Co-authored-by: Lia Stratopoulos <[email protected]>

src/viam/sdk/components/arm.hpp

@@ -5,10 +5,10 @@
 
 #include <string>
 
+#include <boost/optional/optional.hpp>
 #include <boost/variant/variant.hpp>
 
 #include <viam/api/common/v1/common.pb.h>
-#include <viam/api/component/arm/v1/arm.pb.h>
 
 #include <viam/sdk/common/pose.hpp>
 #include <viam/sdk/resource/stoppable.hpp>
@@ -61,6 +61,12 @@ class Arm : public Component, public Stoppable {
     using KinematicsData =
         boost::variant<KinematicsDataUnspecified, KinematicsDataSVA, KinematicsDataURDF>;
 
+    /// @brief Movement specifications for move_through_join_positions.
+    struct MoveOptions {
+        boost::optional<double> max_vel_degs_per_sec;
+        boost::optional<double> 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<double>& 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<std::vector<double>>& 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<std::vector<double>>& positions,
+                                              const MoveOptions& options,
+                                              const ProtoStruct& extra) = 0;
+
     /// @brief Reports if the arm is in motion.
     virtual bool is_moving() = 0;
 

src/viam/sdk/components/private/arm_client.cpp

@@ -46,6 +46,31 @@ void ArmClient::move_to_joint_positions(const std::vector<double>& positions,
         .invoke();
 }
 
+void ArmClient::move_through_joint_positions(const std::vector<std::vector<double>>& 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();

src/viam/sdk/components/private/arm_client.hpp

@@ -26,6 +26,9 @@ class ArmClient : public Arm {
     std::vector<double> get_joint_positions(const ProtoStruct& extra) override;
     void move_to_joint_positions(const std::vector<double>& positions,
                                  const ProtoStruct& extra) override;
+    void move_through_joint_positions(const std::vector<std::vector<double>>& 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;

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<Arm>(
+        "ArmServer::MoveThroughJointPositions", this, request)([&](auto& helper, auto& arm) {
+        std::vector<std::vector<double>> 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 {

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;

src/viam/sdk/tests/mocks/mock_arm.cpp

@@ -1,5 +1,6 @@
 #include <viam/sdk/tests/mocks/mock_arm.hpp>
 
+#include "mock_arm.hpp"
 #include <viam/sdk/tests/test_utils.hpp>
 
 namespace viam {
@@ -31,6 +32,13 @@ void MockArm::move_to_joint_positions(const std::vector<double>& positions,
     joint_positions = positions;
 }
 
+void MockArm::move_through_joint_positions(const std::vector<std::vector<double>>& 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;
 }

src/viam/sdk/tests/mocks/mock_arm.hpp

@@ -19,6 +19,11 @@ class MockArm : public sdk::Arm {
     std::vector<double> get_joint_positions(const sdk::ProtoStruct&) override;
     void move_to_joint_positions(const std::vector<double>& positions,
                                  const sdk::ProtoStruct&) override;
+
+    void move_through_joint_positions(const std::vector<std::vector<double>>& 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<double> joint_positions;
+    std::vector<std::vector<double>> move_thru_positions;
+    sdk::Arm::MoveOptions move_opts;
     bool peek_stop_called;
     sdk::ProtoStruct peek_command;
 };

src/viam/sdk/tests/test_arm.cpp

@@ -1,14 +1,16 @@
 #define BOOST_TEST_MODULE test module test_arm
+#include <viam/sdk/components/arm.hpp>
 
+#include <boost/optional/optional_io.hpp>
 #include <boost/qvm/all.hpp>
 #include <boost/test/included/unit_test.hpp>
 
-#include <viam/sdk/components/arm.hpp>
 #include <viam/sdk/tests/mocks/mock_arm.hpp>
 #include <viam/sdk/tests/test_utils.hpp>
 
 BOOST_TEST_DONT_PRINT_LOG_VALUE(std::vector<viam::sdk::GeometryConfig>)
 BOOST_TEST_DONT_PRINT_LOG_VALUE(std::vector<double>)
+BOOST_TEST_DONT_PRINT_LOG_VALUE(std::vector<std::vector<double>>)
 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<MockArm> mock = MockArm::get_mock_arm();
+    client_to_mock_pipeline<Arm>(mock, [&](Arm& client) {
+        std::vector<std::vector<double>> 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<MockArm> mock = MockArm::get_mock_arm();
     client_to_mock_pipeline<Arm>(mock, [&](Arm& client) {