update arm components

Author: lia-viam

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.
@@ -105,16 +111,16 @@ class Arm : public Component, public Stoppable {
 
     /// @brief Move each joint on the arm through the positions specified in @param positions
     /// @param options optional specifications to be obeyed during the motion.
-    inline void move_through_joint_positions(const std::vector<std::vector<double>>& positions, 
-                                             const component::arm::v1::MoveOptions& options) {
+    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 component::arm::v1::MoveOptions& options, 
+                                              const MoveOptions& options,
                                               const ProtoStruct& extra) = 0;
 
     /// @brief Reports if the arm is in motion.

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

@@ -47,13 +47,26 @@ void ArmClient::move_to_joint_positions(const std::vector<double>& positions,
 }
 
 void ArmClient::move_through_joint_positions(const std::vector<std::vector<double>>& positions,
-                                             const component::arm::v1::MoveOptions& options, 
+                                             const Arm::MoveOptions& options,
                                              const ProtoStruct& extra) {
     return make_client_helper(this, *stub_, &StubType::MoveThroughJointPositions)
         .with(extra,
-                // TODO: dont understand how to write this part
-              [&](auto& request) {
-                  *(request.mutable_positions()->mutable_values()) = {};
+              [&](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 std::vector<double>& pos : positions) {
+                      viam::component::arm::v1::JointPositions jpos;
+                      jpos.mutable_values()->Assign(pos.begin(), pos.end());
+                      request.mutable_positions()->Add(std::move(jpos));
+                  }
               })
         .invoke();
 }

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

@@ -27,7 +27,7 @@ class ArmClient : public Arm {
     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 component::arm::v1::MoveOptions& options, 
+                                      const Arm::MoveOptions& options,
                                       const ProtoStruct& extra) override;
     bool is_moving() override;
     void stop(const ProtoStruct& extra) override;
@@ -41,8 +41,8 @@ class ArmClient : public Arm {
     using Arm::get_geometries;
     using Arm::get_joint_positions;
     using Arm::get_kinematics;
-    using Arm::move_to_joint_positions;
     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

@@ -59,11 +59,28 @@ ::grpc::Status ArmServer::MoveThroughJointPositions(
     ::viam::component::arm::v1::MoveThroughJointPositionsResponse*) noexcept {
     std::vector<std::vector<double>> positions;
     for (int i = 0; i < request->positions_size(); i++) {
-        positions.push_back({request->positions(i).values().begin(), request->positions(i).values().end()});
+        positions.push_back(
+            {request->positions(i).values().begin(), request->positions(i).values().end()});
     }
     return make_service_helper<Arm>(
-        "ArmServer::MoveThroughJointPositions", this, request) ([&](auto& helper, auto& arm) { 
-        arm->move_through_joint_positions(positions, request->options(), helper.getExtra()); 
+        "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.push_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());
     });
 }