From af108e4b56c517e342e15c1137603229618f2a06 Mon Sep 17 00:00:00 2001
From: Florian Walch <florian.walch@franka.de>
Date: Wed, 21 Feb 2018 11:55:00 +0100
Subject: [PATCH] Revert "Remove gripper epsilon"
This reverts commit 0635e31d0634a80b68b11b3c3bb468bfb1af99a2.
---
CHANGELOG.md | 1 +
franka_gripper/CMakeLists.txt | 5 +++++
franka_gripper/action/Grasp.action | 1 +
franka_gripper/config/gripper_node_config.yaml | 3 +++
.../include/franka_gripper/franka_gripper.h | 8 ++++++--
franka_gripper/msg/GraspEpsilon.msg | 2 ++
franka_gripper/src/franka_gripper.cpp | 7 +++++--
franka_gripper/src/franka_gripper_node.cpp | 16 ++++++++++++++--
8 files changed, 37 insertions(+), 6 deletions(-)
create mode 100644 franka_gripper/msg/GraspEpsilon.msg
diff --git a/CHANGELOG.md b/CHANGELOG.md
index 34cbc30..dc512e0 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -5,6 +5,7 @@
Requires `libfranka` >= 0.3.0
* **BREAKING** Changed signatures in `franka_hw::FrankaModelHandle`
+ * **BREAKING** Added epsilon parameters to `franka_gripper/Grasp` action
* Added missing dependency to `panda_moveit_config`
* Fixed linker errors when building with `-DFranka_DIR` while an older version of
`ros-kinetic-libfranka` is installed
diff --git a/franka_gripper/CMakeLists.txt b/franka_gripper/CMakeLists.txt
index bdec200..3055ed4 100644
--- a/franka_gripper/CMakeLists.txt
+++ b/franka_gripper/CMakeLists.txt
@@ -24,6 +24,11 @@ add_action_files(
Move.action
)
+add_message_files(
+ DIRECTORY msg
+ FILES GraspEpsilon.msg
+)
+
generate_messages(DEPENDENCIES actionlib_msgs)
catkin_package(
diff --git a/franka_gripper/action/Grasp.action b/franka_gripper/action/Grasp.action
index 97fa832..a02b952 100644
--- a/franka_gripper/action/Grasp.action
+++ b/franka_gripper/action/Grasp.action
@@ -1,4 +1,5 @@
float64 width # [m]
+GraspEpsilon epsilon
float64 speed # [m/s]
float64 force # [N]
---
diff --git a/franka_gripper/config/gripper_node_config.yaml b/franka_gripper/config/gripper_node_config.yaml
index f7546fd..8799cbd 100644
--- a/franka_gripper/config/gripper_node_config.yaml
+++ b/franka_gripper/config/gripper_node_config.yaml
@@ -3,3 +3,6 @@ joint_names:
- panda_finger_joint1
- panda_finger_joint2
default_speed: 0.1 # [m/s]
+default_grasp_epsilon:
+ inner: 0.005 # [m]
+ outer: 0.005 # [m]
\ No newline at end of file
diff --git a/franka_gripper/include/franka_gripper/franka_gripper.h b/franka_gripper/include/franka_gripper/franka_gripper.h
index 5b9d0b1..6828577 100644
--- a/franka_gripper/include/franka_gripper/franka_gripper.h
+++ b/franka_gripper/include/franka_gripper/franka_gripper.h
@@ -35,11 +35,13 @@ bool updateGripperState(const franka::Gripper& gripper, franka::GripperState* st
*
* @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);
@@ -75,9 +77,11 @@ bool stop(const franka::Gripper& gripper, const StopGoalConstPtr& /*goal*/);
/**
* Calls the libfranka grasp service of the gripper
*
- * @param[in] gripper A gripper instance to execute the command
- * @param[in] goal A grasp goal with target width, velocity and effort
+ * 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);
diff --git a/franka_gripper/msg/GraspEpsilon.msg b/franka_gripper/msg/GraspEpsilon.msg
new file mode 100644
index 0000000..b750cf6
--- /dev/null
+++ b/franka_gripper/msg/GraspEpsilon.msg
@@ -0,0 +1,2 @@
+float64 inner # [m]
+float64 outer # [m]
diff --git a/franka_gripper/src/franka_gripper.cpp b/franka_gripper/src/franka_gripper.cpp
index 5f7ffc6..a58493d 100644
--- a/franka_gripper/src/franka_gripper.cpp
+++ b/franka_gripper/src/franka_gripper.cpp
@@ -29,6 +29,7 @@ bool updateGripperState(const franka::Gripper& gripper, franka::GripperState* st
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) {
@@ -42,7 +43,8 @@ void gripperCommandExecuteCallback(
if (goal->command.position >= state.width) {
return gripper.move(goal->command.position, default_speed);
}
- return gripper.grasp(goal->command.position, default_speed, goal->command.max_effort);
+ return gripper.grasp(goal->command.position, default_speed, goal->command.max_effort,
+ grasp_epsilon.inner, grasp_epsilon.outer);
};
try {
@@ -77,7 +79,8 @@ bool stop(const franka::Gripper& gripper, const StopGoalConstPtr& /*goal*/) {
}
bool grasp(const franka::Gripper& gripper, const GraspGoalConstPtr& goal) {
- return gripper.grasp(goal->width, goal->speed, goal->force);
+ return gripper.grasp(goal->width, goal->speed, goal->force, goal->epsilon.inner,
+ goal->epsilon.outer);
}
} // namespace franka_gripper
diff --git a/franka_gripper/src/franka_gripper_node.cpp b/franka_gripper/src/franka_gripper_node.cpp
index 31ed7c2..8b5b5ea 100644
--- a/franka_gripper/src/franka_gripper_node.cpp
+++ b/franka_gripper/src/franka_gripper_node.cpp
@@ -38,6 +38,7 @@ void handleErrors(actionlib::SimpleActionServer<T_action>* server,
using actionlib::SimpleActionServer;
using control_msgs::GripperCommandAction;
using franka_gripper::GraspAction;
+using franka_gripper::GraspEpsilon;
using franka_gripper::GraspGoalConstPtr;
using franka_gripper::GraspResult;
using franka_gripper::HomingAction;
@@ -70,6 +71,17 @@ int main(int argc, char** argv) {
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);
std::function<bool(const HomingGoalConstPtr&)> homing_handler =
@@ -104,8 +116,8 @@ int main(int argc, char** argv) {
SimpleActionServer<GripperCommandAction> gripper_command_action_server(
node_handle, "gripper_action",
- std::bind(&gripperCommandExecuteCallback, std::cref(gripper), default_speed,
- &gripper_command_action_server, std::placeholders::_1),
+ std::bind(&gripperCommandExecuteCallback, std::cref(gripper), default_grasp_epsilon,
+ default_speed, &gripper_command_action_server, std::placeholders::_1),
false);
homing_action_server_.start();
--
GitLab