Skip to content
Snippets Groups Projects
Commit af108e4b authored by Florian Walch's avatar Florian Walch
Browse files

Revert "Remove gripper epsilon"

This reverts commit 0635e31d.
parent ccc7659a
No related branches found
No related tags found
No related merge requests found
......@@ -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
......
......@@ -24,6 +24,11 @@ add_action_files(
Move.action
)
add_message_files(
DIRECTORY msg
FILES GraspEpsilon.msg
)
generate_messages(DEPENDENCIES actionlib_msgs)
catkin_package(
......
float64 width # [m]
GraspEpsilon epsilon
float64 speed # [m/s]
float64 force # [N]
---
......
......@@ -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
......@@ -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);
......
float64 inner # [m]
float64 outer # [m]
......@@ -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
......@@ -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();
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment