Skip to content

Commit

Permalink
Merge pull request #47 in SRR/franka_ros2 from tests/robot-class to h…
Browse files Browse the repository at this point in the history
…umble

* commit '9f58e2a3966e70f49c7eac8c4901db97314547a0':
  bump up version 0.1.8
  hotfix: include patched JTC back
  tests: implement franka hardware robot class tests
  • Loading branch information
BarisYazici committed Nov 17, 2023
2 parents c335b6b + 9f58e2a commit 1768569
Show file tree
Hide file tree
Showing 42 changed files with 7,252 additions and 31 deletions.
1 change: 0 additions & 1 deletion .devcontainer/Dockerfile
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,6 @@ RUN apt-get update && \
ros-humble-joint-state-broadcaster \
ros-humble-ament-cmake-clang-tidy \
libignition-gazebo6-dev \
ros-humble-joint-trajectory-controller \
libpoco-dev \
libeigen3-dev \
&& rm -rf /var/lib/apt/lists/*
Expand Down
10 changes: 1 addition & 9 deletions .devcontainer/devcontainer.json
Original file line number Diff line number Diff line change
Expand Up @@ -13,15 +13,7 @@
"bungcip.better-toml",
"ms-vscode.cpptools",
"ms-vscode.cpptools-extension-pack"
],
"settings": {
"terminal.integrated.defaultProfile.linux": "bash",
"terminal.integrated.profiles.linux": {
"bash": {
"path": "/bin/bash"
}
}
}
]
}
}
}
9 changes: 8 additions & 1 deletion CHANGELOG.md
Original file line number Diff line number Diff line change
@@ -1,10 +1,17 @@
# Changelog

## 0.1.8 - 2023-11-16

Requires libfranka >= 0.13.0, required ROS 2 Humble

* franka\_hardware: add unit tests for robot class.
* joint\_trajectory\_controller: hotfix add joint patched old JTC back.

## 0.1.7 - 2023-11-10

Requires libfranka >= 0.12.1, required ROS 2 Humble

* franka\_hardware: joint position command inteface supported
* franka\_hardware: joint position command interface supported
* franka\_hardware: controller initializer automatically acknowledges error, if arm is in reflex mode
* franka\_example\_controllers: joint position example controller provided
* franka\_example\_controllers: fix second start bug with the example controllers
Expand Down
2 changes: 1 addition & 1 deletion Dockerfile
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@ RUN python3 -m pip install -U \
RUN mkdir ~/source_code
RUN cd ~/source_code && git clone https://github.com/frankaemika/libfranka.git \
&& cd libfranka \
&& git checkout 0.12.1 \
&& git checkout 0.13.0 \
&& git submodule init \
&& git submodule update \
&& mkdir build && cd build \
Expand Down
2 changes: 1 addition & 1 deletion franka_bringup/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>franka_bringup</name>
<version>0.1.7</version>
<version>0.1.8</version>
<description>Package with launch files and run-time configurations for using Franka Emika research robots with ros2_control</description>
<maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
<license>Apache 2.0</license>
Expand Down
2 changes: 1 addition & 1 deletion franka_description/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>franka_description</name>
<version>0.1.7</version>
<version>0.1.8</version>
<description>franka_description contains URDF files and meshes of Franka Emika robots</description>
<maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
<license>Apache 2.0</license>
Expand Down
2 changes: 1 addition & 1 deletion franka_example_controllers/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>franka_example_controllers</name>
<version>0.1.7</version>
<version>0.1.8</version>
<description>franka_example_controllers provides example code for controlling Franka Emika research robots with ros2_control</description>
<maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
<license>Apache 2.0</license>
Expand Down
2 changes: 1 addition & 1 deletion franka_gripper/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ find_package(std_srvs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(control_msgs REQUIRED)

find_package(Franka REQUIRED)
find_package(Franka 0.13.0 REQUIRED)

add_library(gripper_server SHARED
src/gripper_action_server.cpp)
Expand Down
2 changes: 1 addition & 1 deletion franka_gripper/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>franka_gripper</name>
<version>0.1.7</version>
<version>0.1.8</version>
<description>This package implements the franka gripper of type Franka Hand for the use in ROS2</description>
<maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
<license>Apache 2.0</license>
Expand Down
2 changes: 1 addition & 1 deletion franka_hardware/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ find_package(rclcpp REQUIRED)
find_package(franka_msgs REQUIRED)
find_package(hardware_interface REQUIRED)
find_package(pluginlib REQUIRED)
find_package(Franka 0.12.1 REQUIRED)
find_package(Franka 0.13.0 REQUIRED)
find_package(rclcpp_components REQUIRED)

add_library(franka_hardware
Expand Down
13 changes: 10 additions & 3 deletions franka_hardware/include/franka_hardware/robot.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
#include <thread>

#include <franka/active_control.h>
#include <franka/active_control_base.h>
#include <franka/active_motion_generator.h>
#include <franka/active_torque_control.h>

Expand All @@ -44,6 +45,13 @@ namespace franka_hardware {

class Robot {
public:
/**
* @brief Construct a new Robot object for tests purposes
*
* @param robot mocked libfranka robot
* @param model mocked model object
*/
explicit Robot(std::unique_ptr<franka::Robot> robot, std::unique_ptr<Model> model);
/**
* Connects to the robot. This method can block for up to one minute if the robot is not
* responding. An exception will be thrown if the connection cannot be established.
Expand Down Expand Up @@ -262,17 +270,16 @@ class Robot {
std::mutex control_mutex_;

std::unique_ptr<franka::Robot> robot_;
std::unique_ptr<franka::ActiveControl> active_control_;
std::unique_ptr<franka::ActiveControlBase> active_control_ = nullptr;
std::unique_ptr<franka::Model> model_;
std::unique_ptr<Model> franka_hardware_model_;

std::array<double, 7> last_desired_torque_ = {0, 0, 0, 0, 0, 0, 0};

bool effort_interface_active_{false};
bool joint_velocity_interface_active_{false};
bool joint_position_interface_active_{false};
bool cartesian_velocity_interface_active_{false};

bool torque_command_rate_limiter_active_{true};
bool velocity_command_rate_limit_active_{false};

bool cartesian_velocity_command_rate_limit_active_{false};
Expand Down
2 changes: 1 addition & 1 deletion franka_hardware/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>franka_hardware</name>
<version>0.1.7</version>
<version>0.1.8</version>
<description>franka_hardware provides hardware interfaces for using Franka Emika research robots with ros2_control</description>
<maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
<license>Apache 2.0</license>
Expand Down
22 changes: 18 additions & 4 deletions franka_hardware/src/robot.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,9 @@

namespace franka_hardware {

Robot::Robot(std::unique_ptr<franka::Robot> robot, std::unique_ptr<Model> model)
: robot_(std::move(robot)), franka_hardware_model_(std::move(model)) {}

Robot::Robot(const std::string& robot_ip, const rclcpp::Logger& logger) {
franka::RealtimeConfig rt_config = franka::RealtimeConfig::kEnforce;
if (!franka::hasRealtimeKernel()) {
Expand Down Expand Up @@ -65,6 +68,9 @@ void Robot::stopRobot() {
}

void Robot::writeOnce(const std::array<double, 7>& joint_commands) {
if (!active_control_) {
throw std::runtime_error("Control hasn't been started");
}
if (effort_interface_active_) {
writeOnceEfforts(joint_commands);
} else if (joint_velocity_interface_active_) {
Expand All @@ -78,10 +84,10 @@ void Robot::writeOnceEfforts(const std::array<double, 7>& efforts) {
std::lock_guard<std::mutex> lock(control_mutex_);

auto torque_command = franka::Torques(efforts);
torque_command.tau_J =
franka::limitRate(franka::kMaxTorqueRate, torque_command.tau_J, last_desired_torque_);
last_desired_torque_ = torque_command.tau_J;

if (torque_command_rate_limiter_active_) {
torque_command.tau_J =
franka::limitRate(franka::kMaxTorqueRate, torque_command.tau_J, current_state_.tau_J_d);
}
active_control_->writeOnce(torque_command);
}

Expand Down Expand Up @@ -162,6 +168,10 @@ void Robot::preProcessCartesianVelocities(franka::CartesianVelocities& velocity_
}

void Robot::writeOnce(const std::array<double, 6>& cartesian_velocities) {
if (!active_control_) {
throw std::runtime_error("Control hasn't been started");
}

std::lock_guard<std::mutex> lock(control_mutex_);

auto velocity_command = franka::CartesianVelocities(cartesian_velocities);
Expand All @@ -172,6 +182,10 @@ void Robot::writeOnce(const std::array<double, 6>& cartesian_velocities) {

void Robot::writeOnce(const std::array<double, 6>& cartesian_velocities,
const std::array<double, 2>& elbow_command) {
if (!active_control_) {
throw std::runtime_error("Control hasn't been started");
}

std::lock_guard<std::mutex> lock(control_mutex_);

auto velocity_command = franka::CartesianVelocities(cartesian_velocities, elbow_command);
Expand Down
5 changes: 5 additions & 0 deletions franka_hardware/test/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -10,3 +10,8 @@ ament_add_gmock(${PROJECT_NAME}_command_interface_test franka_hardware_cartesian
target_include_directories(${PROJECT_NAME}_command_interface_test PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../include)

target_link_libraries(${PROJECT_NAME}_command_interface_test ${PROJECT_NAME})

ament_add_gmock(${PROJECT_NAME}_robot_test franka_robot_test.cpp)
target_include_directories(${PROJECT_NAME}_robot_test PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../include)

target_link_libraries(${PROJECT_NAME}_robot_test ${PROJECT_NAME})
149 changes: 149 additions & 0 deletions franka_hardware/test/franka_robot_test.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,149 @@
#include "franka_robot_test.hpp"

TEST_F(FrankaRobotTests, whenInitializeTorqueInterfaceCalled_thenStartTorqueControlCalled) {
EXPECT_CALL(*mock_libfranka_robot, startTorqueControl()).Times(1);

franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model));

robot.initializeTorqueInterface();
}

TEST_F(FrankaRobotTests, whenInitializeJointVelocityInterfaceCalled_thenStartJointVelocityControl) {
EXPECT_CALL(*mock_libfranka_robot, startJointVelocityControl(testing::_)).Times(1);

franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model));

robot.initializeJointVelocityInterface();
}

TEST_F(FrankaRobotTests,
whenInitializeJointtPositionInterfaceCalled_thenStartJointPositionControl) {
EXPECT_CALL(*mock_libfranka_robot, startJointPositionControl(testing::_)).Times(1);

franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model));

robot.initializeJointPositionInterface();
}

TEST_F(FrankaRobotTests,
whenInitializeCartesianVelocityInterfaceCalled_thenStartCartesianVelocityControl) {
EXPECT_CALL(*mock_libfranka_robot, startCartesianVelocityControl(testing::_)).Times(1);

franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model));

robot.initializeCartesianVelocityInterface();
}

TEST_F(FrankaRobotTests,
givenCartesianVelocityControlIsStarted_whenReadOnceIsCalled_expectCorrectRobotState) {
franka::RobotState robot_state;
franka::Duration duration;
robot_state.q_d = std::array<double, 7>{1, 2, 3, 1, 2, 3, 1};
auto active_control_read_return_tuple = std::make_pair(robot_state, duration);

EXPECT_CALL(*mock_active_control, readOnce())
.WillOnce(testing::Return(active_control_read_return_tuple));
EXPECT_CALL(*mock_libfranka_robot, startCartesianVelocityControl(testing::_))
.WillOnce(testing::Return(testing::ByMove((std::move(mock_active_control)))));

franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model));

robot.initializeCartesianVelocityInterface();
auto actual_state = robot.readOnce();
ASSERT_EQ(robot_state.q_d, actual_state.q_d);
}

TEST_F(FrankaRobotTests,
givenJointControlIsNotStarted_whenReadOnceIsCalled_expectCorrectRobotState) {
franka::RobotState robot_state;
robot_state.q_d = std::array<double, 7>{1, 2, 3, 1, 2, 3, 1};

EXPECT_CALL(*mock_libfranka_robot, readOnce()).WillOnce(testing::Return(robot_state));

franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model));
auto actual_state = robot.readOnce();

ASSERT_EQ(robot_state.q_d, actual_state.q_d);
}

TEST_F(
FrankaRobotTests,
givenCartesianVelocityControlIsStarted_whenWriteOnceIsCalled_expectActiveControlWriteOnceCalled) {
const std::array<double, 6>& cartesian_velocities{1, 0, 0, 0, 0, 0};
const franka::CartesianVelocities expected_cartesian_velocities(cartesian_velocities);

auto expectCallFunction = [this]() {
EXPECT_CALL(*mock_libfranka_robot, startCartesianVelocityControl(testing::_))
.WillOnce(testing::Return(testing::ByMove((std::move(mock_active_control)))));
};

testWriteOnce<void (franka_hardware::Robot::*)(), franka::CartesianVelocities,
std::array<double, 6>>(
&franka_hardware::Robot::initializeCartesianVelocityInterface, expectCallFunction,
cartesian_velocities, expected_cartesian_velocities);
}

TEST_F(
FrankaRobotTests,
givenJointPositionControlIsControlIsStarted_whenWriteOnceIsCalled_expectActiveControlWriteOnceCalled) {
const std::array<double, 7>& joint_positions{1, 0, 0, 0, 0, 0, 0};
const franka::JointPositions expected_joint_positions(joint_positions);

auto expectCallFunction = [this]() {
EXPECT_CALL(*mock_libfranka_robot, startJointPositionControl(testing::_))
.WillOnce(testing::Return(testing::ByMove((std::move(mock_active_control)))));
};

testWriteOnce<void (franka_hardware::Robot::*)(), franka::JointPositions, std::array<double, 7>>(
&franka_hardware::Robot::initializeJointPositionInterface, expectCallFunction,
joint_positions, expected_joint_positions);
}

TEST_F(
FrankaRobotTests,
givenJointVelocityControlIsStarted_whenWriteOnceIsCalled_expectActiveControlWriteOnceCalled) {
const std::array<double, 7>& joint_velocities{1, 0, 0, 0, 0, 0, 0};
const franka::JointVelocities expected_joint_velocities(joint_velocities);

const auto expectCallFunction = [this]() {
EXPECT_CALL(*mock_libfranka_robot, startJointVelocityControl(testing::_))
.WillOnce(testing::Return(testing::ByMove((std::move(mock_active_control)))));
};

testWriteOnce<void (franka_hardware::Robot::*)(), franka::JointVelocities, std::array<double, 7>>(
&franka_hardware::Robot::initializeJointVelocityInterface, expectCallFunction,
joint_velocities, expected_joint_velocities);
}

TEST_F(FrankaRobotTests,
givenEffortControlIsStarted_whenWriteOnceIsCalled_expectActiveControlWriteOnceCalled) {
const std::array<double, 7>& joint_torques{1, 0, 0, 0, 0, 0, 0};
// Torque rate limiter defaulted to active
const franka::Torques expected_joint_torques{0.999999, 0, 0, 0, 0, 0, 0};

auto expectCallFunction = [this]() {
EXPECT_CALL(*mock_libfranka_robot, startTorqueControl())
.WillOnce(testing::Return(testing::ByMove((std::move(mock_active_control)))));
};

testWriteOnce<void (franka_hardware::Robot::*)(), franka::Torques, std::array<double, 7>>(
&franka_hardware::Robot::initializeTorqueInterface, expectCallFunction, joint_torques,
expected_joint_torques);
}

TEST_F(FrankaRobotTests,
givenControlIsNotStart_whenWriteOnceIsCalled_expectRuntimeExceptionToBeThrown) {
const std::array<double, 7>& joint_torques{1, 0, 0, 0, 0, 0, 0};
const franka::Torques joint_torques_franka(joint_torques);

const std::array<double, 6>& cartesian_velocities{1, 0, 0, 0, 0, 0};
const franka::CartesianVelocities cartesian_franka(cartesian_velocities);

EXPECT_CALL(*mock_active_control, writeOnce(joint_torques_franka)).Times(0);
EXPECT_CALL(*mock_active_control, writeOnce(cartesian_franka)).Times(0);

franka_hardware::Robot robot(std::move(mock_libfranka_robot), std::move(mock_model));

EXPECT_THROW(robot.writeOnce(joint_torques), std::runtime_error);
EXPECT_THROW(robot.writeOnce(cartesian_velocities), std::runtime_error);
}
Loading

0 comments on commit 1768569

Please sign in to comment.