Skip to content
Snippets Groups Projects
Commit 59a6a9a8 authored by Sebastian Ebert's avatar Sebastian Ebert
Browse files

original version

parents
No related branches found
No related tags found
No related merge requests found
cmake_minimum_required(VERSION 3.4)
project(franka_gripper)
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
find_package(catkin REQUIRED COMPONENTS
roscpp
message_generation
control_msgs
actionlib
sensor_msgs
xmlrpcpp
actionlib_msgs
)
find_package(Franka 0.7.0 REQUIRED)
add_action_files(
DIRECTORY action
FILES Grasp.action
Homing.action
Stop.action
Move.action
)
add_message_files(
DIRECTORY msg
FILES GraspEpsilon.msg
)
generate_messages(DEPENDENCIES actionlib_msgs)
catkin_package(
INCLUDE_DIRS include
LIBRARIES franka_gripper
CATKIN_DEPENDS roscpp
message_runtime
control_msgs
actionlib
sensor_msgs
xmlrpcpp
actionlib_msgs
DEPENDS Franka
)
add_library(franka_gripper
src/franka_gripper.cpp
)
add_dependencies(franka_gripper
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
${PROJECT_NAME}_generate_messages_cpp
)
target_link_libraries(franka_gripper
${Franka_LIBRARIES}
${catkin_LIBRARIES}
)
target_include_directories(franka_gripper SYSTEM PUBLIC
${Franka_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)
target_include_directories(franka_gripper PUBLIC
include
)
add_executable(franka_gripper_node
src/franka_gripper_node.cpp
)
add_dependencies(franka_gripper_node
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
franka_gripper
)
target_link_libraries(franka_gripper_node
${catkin_LIBRARIES}
franka_gripper
)
target_include_directories(franka_gripper_node SYSTEM PUBLIC
${catkin_INCLUDE_DIRS}
)
## Installation
install(TARGETS franka_gripper
franka_gripper_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY config
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
## Tools
include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL
RESULT_VARIABLE CLANG_TOOLS
)
if(CLANG_TOOLS)
file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp)
file(GLOB_RECURSE HEADERS
${CMAKE_CURRENT_SOURCE_DIR}/include/*.h
${CMAKE_CURRENT_SOURCE_DIR}/src/*.h
)
add_format_target(franka_gripper FILES ${SOURCES} ${HEADERS})
add_tidy_target(franka_gripper
FILES ${SOURCES}
DEPENDS franka_gripper franka_gripper_node
)
endif()
float64 width # [m]
GraspEpsilon epsilon
float64 speed # [m/s]
float64 force # [N]
---
bool success
string error
---
---
bool success
string error
---
float64 width # [m]
float64 speed # [m/s]
---
bool success
string error
---
---
bool success
string error
---
publish_rate: 30 # [Hz]
default_speed: 0.1 # [m/s]
default_grasp_epsilon:
inner: 0.005 # [m]
outer: 0.005 # [m]
// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#pragma once
#include <cmath>
#include <string>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/GripperCommandAction.h>
#include <ros/node_handle.h>
#include <franka/exception.h>
#include <franka/gripper.h>
#include <franka/gripper_state.h>
#include <franka_gripper/GraspAction.h>
#include <franka_gripper/HomingAction.h>
#include <franka_gripper/MoveAction.h>
#include <franka_gripper/StopAction.h>
namespace franka_gripper {
/**
* Reads a gripper state if possible
*
* @param[in] state A gripper state to update
* @param[in] gripper A pointer to a franka gripper
*
* @return True if update was successful, false otherwise.
*/
bool updateGripperState(const franka::Gripper& gripper, franka::GripperState* state);
/**
* Wraps the execution of a gripper command action to catch exceptions and
* report results
*
* @note
* For compatibility with current MoveIt! behavior, the given goal's command position is
* multiplied by a factor of 2 before being commanded to the gripper!
*
* @param[in] gripper A pointer to a franka gripper
* @param[in] default_speed The default speed for a gripper action
* @param[in] grasp_epsilon The epsilon window of the grasp.
* @param[in] action_server A pointer to a gripper action server
* @param[in] goal A gripper action goal
*/
void gripperCommandExecuteCallback(
const franka::Gripper& gripper,
const GraspEpsilon& grasp_epsilon,
double default_speed,
actionlib::SimpleActionServer<control_msgs::GripperCommandAction>* action_server,
const control_msgs::GripperCommandGoalConstPtr& goal);
/**
* Calls the libfranka move service of the gripper
*
* @param[in] gripper A gripper instance to execute the command
* @param[in] goal A move goal with target width and velocity
*
* @return True if command was successful, false otherwise.
*/
bool move(const franka::Gripper& gripper, const MoveGoalConstPtr& goal);
/**
* Calls the libfranka homing service of the gripper
*
* @param[in] gripper A gripper instance to execute the command
*
* @return True if command was successful, false otherwise.
*/
bool homing(const franka::Gripper& gripper, const HomingGoalConstPtr& /*goal*/);
/**
* Calls the libfranka stop service of the gripper to stop applying force
*
* @param[in] gripper A gripper instance to execute the command
*
* @return True if command was successful, false otherwise.
*/
bool stop(const franka::Gripper& gripper, const StopGoalConstPtr& /*goal*/);
/**
* Calls the libfranka grasp service of the gripper
*
* An object is considered grasped if the distance \f$d\f$ between the gripper fingers satisfies
* \f$(\text{width} - \text{epsilon_inner}) < d < (\text{width} + \text{epsilon_outer})\f$.
*
* @param[in] gripper A gripper instance to execute the command
* @param[in] goal A grasp goal with target width, epsilon_inner, epsilon_outer, velocity and effort
* @return True if an object has been grasped, false otherwise.
*/
bool grasp(const franka::Gripper& gripper, const GraspGoalConstPtr& goal);
} // namespace franka_gripper
<?xml version="1.0" ?>
<launch>
<arg name="robot_ip" />
<arg name="arm_id" default="panda" />
<arg name="joint_names" default="[$(arg arm_id)_finger_joint1, $(arg arm_id)_finger_joint2]" />
<node name="franka_gripper" pkg="franka_gripper" type="franka_gripper_node" output="screen">
<param name="robot_ip" value="$(arg robot_ip)"/>
<rosparam command="load" file="$(find franka_gripper)/config/franka_gripper_node.yaml" />
<rosparam param="joint_names" subst_value="true">$(arg joint_names)</rosparam>
</node>
</launch>
/**
* @mainpage
* @htmlinclude "manifest.html"
*
* Overview page for Franka Emika research robots: https://frankaemika.github.io
*/
float64 inner # [m]
float64 outer # [m]
<?xml version="1.0"?>
<package format="2">
<name>franka_gripper</name>
<version>0.7.0</version>
<description>This package implements the franka gripper of type Franka Hand for the use in ros</description>
<maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
<license>Apache 2.0</license>
<url type="website">http://wiki.ros.org/franka_gripper</url>
<url type="repository">https://github.com/frankaemika/franka_ros</url>
<url type="bugtracker">https://github.com/frankaemika/franka_ros/issues</url>
<author>Franka Emika GmbH</author>
<buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
<depend>message_generation</depend>
<depend>libfranka</depend>
<depend>control_msgs</depend>
<depend>actionlib</depend>
<depend>sensor_msgs</depend>
<depend>xmlrpcpp</depend>
<depend>actionlib_msgs</depend>
<exec_depend>message_runtime</exec_depend>
<build_export_depend>message_runtime</build_export_depend>
</package>
- builder: doxygen
javadoc_autobrief: YES
// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <franka_gripper/franka_gripper.h>
#include <cmath>
#include <functional>
#include <thread>
#include <ros/node_handle.h>
#include <franka/exception.h>
#include <franka/gripper_state.h>
#include <franka_gripper/GraspAction.h>
#include <franka_gripper/HomingAction.h>
#include <franka_gripper/MoveAction.h>
#include <franka_gripper/StopAction.h>
namespace franka_gripper {
bool updateGripperState(const franka::Gripper& gripper, franka::GripperState* state) {
try {
*state = gripper.readOnce();
} catch (const franka::Exception& ex) {
ROS_ERROR_STREAM("GripperServer: Exception reading gripper state: " << ex.what());
return false;
}
return true;
}
void gripperCommandExecuteCallback(
const franka::Gripper& gripper,
const GraspEpsilon& grasp_epsilon,
double default_speed,
actionlib::SimpleActionServer<control_msgs::GripperCommandAction>* action_server,
const control_msgs::GripperCommandGoalConstPtr& goal) {
auto gripper_command_handler = [goal, grasp_epsilon, default_speed, &gripper]() {
// HACK: As one gripper finger is <mimic>, MoveIt!'s trajectory execution manager
// only sends us the width of one finger. Multiply by 2 to get the intended width.
double target_width = 2 * goal->command.position;
franka::GripperState state = gripper.readOnce();
if (target_width > state.max_width || target_width < 0.0) {
ROS_ERROR_STREAM("GripperServer: Commanding out of range width! max_width = "
<< state.max_width << " command = " << target_width);
return false;
}
constexpr double kSamePositionThreshold = 1e-4;
if (std::abs(target_width - state.width) < kSamePositionThreshold) {
return true;
}
if (target_width >= state.width) {
return gripper.move(target_width, default_speed);
}
return gripper.grasp(target_width, default_speed, goal->command.max_effort, grasp_epsilon.inner,
grasp_epsilon.outer);
};
try {
if (gripper_command_handler()) {
franka::GripperState state;
if (updateGripperState(gripper, &state)) {
control_msgs::GripperCommandResult result;
result.effort = 0.0;
result.position = state.width;
result.reached_goal = static_cast<decltype(result.reached_goal)>(true);
result.stalled = static_cast<decltype(result.stalled)>(false);
action_server->setSucceeded(result);
return;
}
}
} catch (const franka::Exception& ex) {
ROS_ERROR_STREAM("" << ex.what());
}
action_server->setAborted();
}
bool move(const franka::Gripper& gripper, const MoveGoalConstPtr& goal) {
return gripper.move(goal->width, goal->speed);
}
bool homing(const franka::Gripper& gripper, const HomingGoalConstPtr& /*goal*/) {
return gripper.homing();
}
bool stop(const franka::Gripper& gripper, const StopGoalConstPtr& /*goal*/) {
return gripper.stop();
}
bool grasp(const franka::Gripper& gripper, const GraspGoalConstPtr& goal) {
return gripper.grasp(goal->width, goal->speed, goal->force, goal->epsilon.inner,
goal->epsilon.outer);
}
} // namespace franka_gripper
// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <functional>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
#include <ros/init.h>
#include <ros/node_handle.h>
#include <ros/rate.h>
#include <ros/spinner.h>
#include <sensor_msgs/JointState.h>
#include <franka/gripper_state.h>
#include <franka_gripper/franka_gripper.h>
namespace {
template <typename T_action, typename T_goal, typename T_result>
void handleErrors(actionlib::SimpleActionServer<T_action>* server,
std::function<bool(const T_goal&)> handler,
const T_goal& goal) {
T_result result;
try {
result.success = handler(goal);
server->setSucceeded(result);
} catch (const franka::Exception& ex) {
ROS_ERROR_STREAM("" << ex.what());
result.success = false;
result.error = ex.what();
server->setAborted(result);
}
}
} // anonymous namespace
using actionlib::SimpleActionServer;
using control_msgs::GripperCommandAction;
using franka_gripper::grasp;
using franka_gripper::GraspAction;
using franka_gripper::GraspEpsilon;
using franka_gripper::GraspGoalConstPtr;
using franka_gripper::GraspResult;
using franka_gripper::gripperCommandExecuteCallback;
using franka_gripper::homing;
using franka_gripper::HomingAction;
using franka_gripper::HomingGoalConstPtr;
using franka_gripper::HomingResult;
using franka_gripper::move;
using franka_gripper::MoveAction;
using franka_gripper::MoveGoalConstPtr;
using franka_gripper::MoveResult;
using franka_gripper::stop;
using franka_gripper::StopAction;
using franka_gripper::StopGoalConstPtr;
using franka_gripper::StopResult;
using franka_gripper::updateGripperState;
int main(int argc, char** argv) {
ros::init(argc, argv, "franka_gripper_node");
ros::NodeHandle node_handle("~");
std::string robot_ip;
if (!node_handle.getParam("robot_ip", robot_ip)) {
ROS_ERROR("franka_gripper_node: Could not parse robot_ip parameter");
return -1;
}
double default_speed(0.1);
if (node_handle.getParam("default_speed", default_speed)) {
ROS_INFO_STREAM("franka_gripper_node: Found default_speed " << default_speed);
}
GraspEpsilon default_grasp_epsilon;
default_grasp_epsilon.inner = 0.005;
default_grasp_epsilon.outer = 0.005;
std::map<std::string, double> epsilon_map;
if (node_handle.getParam("default_grasp_epsilon", epsilon_map)) {
ROS_INFO_STREAM("franka_gripper_node: Found default_grasp_epsilon "
<< "inner: " << epsilon_map["inner"] << ", outer: " << epsilon_map["outer"]);
default_grasp_epsilon.inner = epsilon_map["inner"];
default_grasp_epsilon.outer = epsilon_map["outer"];
}
franka::Gripper gripper(robot_ip);
auto homing_handler = [&gripper](auto&& goal) { return homing(gripper, goal); };
auto stop_handler = [&gripper](auto&& goal) { return stop(gripper, goal); };
auto grasp_handler = [&gripper](auto&& goal) { return grasp(gripper, goal); };
auto move_handler = [&gripper](auto&& goal) { return move(gripper, goal); };
SimpleActionServer<HomingAction> homing_action_server(
node_handle, "homing",
[=, &homing_action_server](auto&& goal) {
return handleErrors<franka_gripper::HomingAction, franka_gripper::HomingGoalConstPtr,
franka_gripper::HomingResult>(&homing_action_server, homing_handler,
goal);
},
false);
SimpleActionServer<StopAction> stop_action_server(
node_handle, "stop",
[=, &stop_action_server](auto&& goal) {
return handleErrors<franka_gripper::StopAction, franka_gripper::StopGoalConstPtr,
franka_gripper::StopResult>(&stop_action_server, stop_handler, goal);
},
false);
SimpleActionServer<MoveAction> move_action_server(
node_handle, "move",
[=, &move_action_server](auto&& goal) {
return handleErrors<franka_gripper::MoveAction, franka_gripper::MoveGoalConstPtr,
franka_gripper::MoveResult>(&move_action_server, move_handler, goal);
},
false);
SimpleActionServer<GraspAction> grasp_action_server(
node_handle, "grasp",
[=, &grasp_action_server](auto&& goal) {
return handleErrors<franka_gripper::GraspAction, franka_gripper::GraspGoalConstPtr,
franka_gripper::GraspResult>(&grasp_action_server, grasp_handler, goal);
},
false);
SimpleActionServer<GripperCommandAction> gripper_command_action_server(
node_handle, "gripper_action",
[=, &gripper, &gripper_command_action_server](auto&& goal) {
return gripperCommandExecuteCallback(gripper, default_grasp_epsilon, default_speed,
&gripper_command_action_server, goal);
},
false);
homing_action_server.start();
stop_action_server.start();
move_action_server.start();
grasp_action_server.start();
gripper_command_action_server.start();
double publish_rate(30.0);
if (!node_handle.getParam("publish_rate", publish_rate)) {
ROS_INFO_STREAM("franka_gripper_node: Could not find parameter publish_rate. Defaulting to "
<< publish_rate);
}
std::vector<std::string> joint_names;
if (!node_handle.getParam("joint_names", joint_names)) {
ROS_ERROR("franka_gripper_node: Could not parse joint_names!");
return -1;
}
if (joint_names.size() != 2) {
ROS_ERROR("franka_gripper_node: Got wrong number of joint_names!");
return -1;
}
franka::GripperState gripper_state;
std::mutex gripper_state_mutex;
std::thread read_thread([&gripper_state, &gripper, &gripper_state_mutex]() {
ros::Rate read_rate(10);
while (ros::ok()) {
{
std::lock_guard<std::mutex> _(gripper_state_mutex);
updateGripperState(gripper, &gripper_state);
}
read_rate.sleep();
}
});
ros::Publisher gripper_state_publisher =
node_handle.advertise<sensor_msgs::JointState>("joint_states", 1);
ros::AsyncSpinner spinner(2);
spinner.start();
ros::Rate rate(publish_rate);
while (ros::ok()) {
if (gripper_state_mutex.try_lock()) {
sensor_msgs::JointState joint_states;
joint_states.header.stamp = ros::Time::now();
joint_states.name.push_back(joint_names[0]);
joint_states.name.push_back(joint_names[1]);
joint_states.position.push_back(gripper_state.width * 0.5);
joint_states.position.push_back(gripper_state.width * 0.5);
joint_states.velocity.push_back(0.0);
joint_states.velocity.push_back(0.0);
joint_states.effort.push_back(0.0);
joint_states.effort.push_back(0.0);
gripper_state_publisher.publish(joint_states);
gripper_state_mutex.unlock();
}
rate.sleep();
}
read_thread.join();
return 0;
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment