Controlling a modular robot gripper

Source code

NOTE: Work in progress. We're continuously improving the documentation, stay tuned for updates

This section will explain the topics and services availables in a H-ROS gripper.

Available topics in a H-ROS gripper:

/hrim_actuator_gripper_000000000004/id
/hrim_actuator_gripper_000000000004/module_3d
/hrim_actuator_gripper_000000000004/module_urdf
/hrim_actuator_gripper_000000000004/power
/hrim_actuator_gripper_000000000004/specs
/hrim_actuator_gripper_000000000004/specs_comm
/hrim_actuator_gripper_000000000004/state_comm
/hrim_actuator_gripper_000000000004/state_finger_gripper
/hrim_actuator_gripper_000000000004/state_gripper
/hrim_actuator_gripper_000000000004/status

For example, if we want to know the position of the fingers we can echo the topic /hrim_actuator_gripper_000000000004/state_finger_gripper:

ros2 topic echo /hrim_actuator_gripper_000000000004/state_finger_gripper
header:
  stamp:
    sec: 7653
    nanosec: 585000000
  frame_id: ''
angular_position: 0.6908019781112671
linear_position: 0.0

Available services in a H-ROS finger gripper:

/hrim_actuator_gripper_000000000004/goal

This service allows us to move the gripper. You can use ROS 2.0 command line tools to open or close the gripper. To fully close the gripper just type:

ros2 service call /hrim_actuator_gripper_000000000004/goal hrim_actuator_gripper_srvs/ControlFinger "goal_angularposition: 0.87"

To fully open the gripper you should type:

ros2 service call /hrim_actuator_gripper_000000000004/goal hrim_actuator_gripper_srvs/ControlFinger "goal_angularposition: 0.0"

This section will explain how to call the service of a H-ROS gripper with a Python script and C++.

#ROS 2.0
from hrim_actuator_gripper_srvs.srv import ControlFinger
import rclpy

rclpy.init(args=None)
node = rclpy.create_node('test_finger_control_service')

cli = node.create_client(ControlFinger, '/hrim_actuator_gripper_000000000004/goal')

req = ControlFinger.Request()

req.goal_velocity = 30.0         # velocity range: 30 -  250 mm/s
req.goal_effort = 10.0        # forces range:   10 -  125 N
req.goal_angularposition = 0.87  # position range   0 - 0.87 rad

future = cli.call_async(req)
rclpy.spin_until_future_complete(node, future)
if future.result() is not None:
    node.get_logger().info('Goal accepted: %d: ' % future.result().goal_accepted)
else:
    node.get_logger().error('Exception while calling service: %r' % future.exception())

ros2 run gripper_minimal_service gripper_minimal_client.py

#include <chrono>
#include <iostream>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "rcutils/cmdline_parser.h"

#include "hrim_actuator_gripper_srvs/srv/control_finger.hpp"

using namespace std::chrono_literals;

void print_usage()
{
  printf("Usage for gripper_minimal_client app:\n");
  printf("gripper_minimal_client [-p position value] [-t topic] [-h]\n");
  printf("options:\n");
  printf("-h : Print this help function.\n");
  printf("-p position value : Position of the fingers.\n");
  printf("-t topic : Name of the service.\n");
}

hrim_actuator_gripper_srvs::srv::ControlFinger::Response::SharedPtr send_request(
  rclcpp::Node::SharedPtr node,
  rclcpp::Client<hrim_actuator_gripper_srvs::srv::ControlFinger>::SharedPtr client,
  hrim_actuator_gripper_srvs::srv::ControlFinger::Request::SharedPtr request)
{
  auto result = client->async_send_request(request);
  // Wait for the result.
  if (rclcpp::spin_until_future_complete(node, result) ==
    rclcpp::executor::FutureReturnCode::SUCCESS)
  {
    return result.get();
  } else {
    return NULL;
  }
}

int main(int argc, char ** argv)
{
  // Force flush of the stdout buffer.
  setvbuf(stdout, NULL, _IONBF, BUFSIZ);

  rclcpp::init(argc, argv);

  auto node = rclcpp::Node::make_shared("hrim_actuator_gripper_srvs");

  if (rcutils_cli_option_exist(argv, argv + argc, "-h")) {
    print_usage();
    return 0;
  }

  float position = 0.0;
  char * cli_option = rcutils_cli_get_option(argv, argv + argc, "-p");
  if (nullptr != cli_option) {
    position = std::atof(cli_option);
  }

  std::string topic("goal");
  char * cli_option_topic = rcutils_cli_get_option(argv, argv + argc, "-t");
  if (nullptr != cli_option_topic) {
    topic = std::string(cli_option_topic);
  }

  auto client = node->create_client<hrim_actuator_gripper_srvs::srv::ControlFinger>(topic);

  auto request = std::make_shared<hrim_actuator_gripper_srvs::srv::ControlFinger::Request>();
  request->goal_angularposition = position;

  while (!client->wait_for_service(1s)) {
    if (!rclcpp::ok()) {
      RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service. Exiting.");
      return 0;
    }
    RCLCPP_INFO(node->get_logger(), "service not available, waiting again...");
  }

  auto result = send_request(node, client, request);
  if (result) {
    RCLCPP_INFO(node->get_logger(), "Result of add_two_ints: %zd", result->goal_accepted);
  } else {
    RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for response. Exiting.");
  }

  rclcpp::shutdown();
  return 0;
}

cd ~/ros2_mara_ws/source
git clone https://github.com/AcutronicRobotics/mara_examples
cd ~/ros2_mara_ws/
colcon build --merge-install --packages-select gripper_minimal_service

ros2 run gripper_minimal_service gripper_minimal_service -h
Usage for gripper_minimal_client app:
gripper_minimal_client [-p position value] [-t topic] [-h]
options:
-h : Print this help function.
-p position value : Position of the fingers.
-t topic : Name of the service.

For example:

ros2 run gripper_minimal_service gripper_minimal_service -t  /hrim_actuator_gripper_000000000004/goal -p 0.85

There is a script inside the mara_utils_scripts pkg that allows us to control the position of the gripper:

source ~/ros2_mara_ws/install/setup.bash
ros2 run mara_utils_scripts gripper_interface.py