Skip to content

RSDK-9291 Add MoveThroughJointPositions to Arm component #324

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 14 commits into from
Nov 18, 2024
Merged

RSDK-9291 Add MoveThroughJointPositions to Arm component #324

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
14 commits
Select commit Hold shift + click to select a range
fd1a03b
first pass at adding new arm endpoint
raybjork Nov 14, 2024
97cfba9
remove line
raybjork Nov 14, 2024
ae21c2c
update arm components
lia-viam Nov 14, 2024
8be6465
add tests
lia-viam Nov 14, 2024
fb87828
Merge branch 'main' of github.com:viamrobotics/viam-cpp-sdk into move…
lia-viam Nov 14, 2024
9309563
lint format
lia-viam Nov 14, 2024
d80a02b
remove original implementation
lia-viam Nov 14, 2024
ac9a0b6
change construction
lia-viam Nov 15, 2024
b5cf377
use add
lia-viam Nov 15, 2024
16d59c6
remove unused var decl
lia-viam Nov 15, 2024
30b77de
use const auto&
lia-viam Nov 18, 2024
0ecd214
use emplace_back
lia-viam Nov 18, 2024
df26f4e
add todo comment
lia-viam Nov 18, 2024
978ad1d
Merge branch 'main' into movethroughjointpos
lia-viam Nov 18, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 23 additions & 1 deletion src/viam/sdk/components/arm.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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,
Copy link
Member

@acmorrow acmorrow Nov 15, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Elsewhere in the SDK, we use xtensor to represent multi-dimensional arrays. It might be worth considering doing that here, as vector<vector isn't the easiest type to work with (for instance, there is no assurance that the inner vectors are all the same length, if that constraint applies here).

Copy link
Contributor Author

@raybjork raybjork Nov 15, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I did this because I'm new to C++ and thought this would be an easy solution. Tensors sound much better I think

Copy link
Member

@acmorrow acmorrow Nov 15, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Have a look at what the mlmodel stuff does with xtensor. Don't be alarmed it if looks a little intense, beausec it is doing something much more complex than what you need since the dimensionality and datatypes can all vary there.

Copy link
Collaborator

@lia-viam lia-viam Nov 18, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

made the other changes but I'm going to save this one for later as a TODO in the interest of unblocking development on this module for now so it can land in this week's release

@raybjork for posterity, is it the case that that all the inner vectors are the same length? and is that something we potentially know in advance for either this UR arm or for a particular arm device?

Copy link
Contributor Author

@raybjork raybjork Nov 18, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think technically a user would be able to send an array of arrays of different lengths, but doing so would be incorrect and should cause an error to be thrown imo. So I think using tensors here does make sense.

You could maybe derive the size of the inner vector by looking at the kinematics file but doing this is outside the scope of the SDK functions and enforcing the correct length should be left to the driver instead.

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;

Expand Down
25 changes: 25 additions & 0 deletions src/viam/sdk/components/private/arm_client.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
4 changes: 4 additions & 0 deletions src/viam/sdk/components/private/arm_client.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand Down
26 changes: 26 additions & 0 deletions src/viam/sdk/components/private/arm_server.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down
5 changes: 5 additions & 0 deletions src/viam/sdk/components/private/arm_server.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
8 changes: 8 additions & 0 deletions src/viam/sdk/tests/mocks/mock_arm.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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 {
Expand Down Expand Up @@ -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;
}
Expand Down
7 changes: 7 additions & 0 deletions src/viam/sdk/tests/mocks/mock_arm.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
};
Expand Down
15 changes: 14 additions & 1 deletion src/viam/sdk/tests/test_arm.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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 {
Expand Down Expand Up @@ -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) {
Expand Down