diff --git a/CHANGELOG.md b/CHANGELOG.md index d1d879a8706e76f77119a42025a5cfe61cc43167..3ac9632ebdc615e7ca250fc29106399c9cf7f47a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -3,7 +3,6 @@ ## 0.2.0 - UNRELEASED * Added missing run-time dependencies to `franka_description` and `franka_control` - * Added epsilon to grasp action * Added `m_ee`, `F_x_Cee`, `I_ee`, `m_total`, `F_x_Ctotal` and `I_total` to the robot state * Updated and improved examples diff --git a/franka_gripper/CMakeLists.txt b/franka_gripper/CMakeLists.txt index 04331c4fd79a80a877944271496554d98eb06821..d0113f202553cf9833103f6ba98b94686af29676 100644 --- a/franka_gripper/CMakeLists.txt +++ b/franka_gripper/CMakeLists.txt @@ -24,11 +24,6 @@ add_action_files( Move.action ) -add_message_files( - DIRECTORY msg - FILES GraspEpsilon.msg -) - generate_messages(DEPENDENCIES actionlib_msgs) catkin_package( @@ -45,7 +40,7 @@ catkin_package( ) add_library(franka_gripper -src/franka_gripper.cpp + src/franka_gripper.cpp ) add_dependencies(franka_gripper diff --git a/franka_gripper/action/Grasp.action b/franka_gripper/action/Grasp.action index a02b952721533ea4fadc9a0df65789f1068cb5b5..97fa8320b9378b93c372611a3ae236ac2079fa81 100644 --- a/franka_gripper/action/Grasp.action +++ b/franka_gripper/action/Grasp.action @@ -1,5 +1,4 @@ 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 8799cbd639f4716f07d550162945c3a3bda08d7e..f7546fd098747578a034b6cce24ce39469a4b146 100644 --- a/franka_gripper/config/gripper_node_config.yaml +++ b/franka_gripper/config/gripper_node_config.yaml @@ -3,6 +3,3 @@ 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 6a80dc297a18d7e975cec6c8429213ca15a64c82..5b9d0b16ba24fe1a30a2c9f5dd20bef2b2091937 100644 --- a/franka_gripper/include/franka_gripper/franka_gripper.h +++ b/franka_gripper/include/franka_gripper/franka_gripper.h @@ -35,13 +35,11 @@ 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); @@ -78,10 +76,9 @@ 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, epsilon_inner, epsilon_outer, velocity and effort + * @param[in] goal A grasp goal with target width, velocity and effort * - * @return True if an object has been grasped, i.e.: the distance between the gripper fingers is - * (width - epsilon_inner) < distance < (width + epsilon_outer), false otherwise. + * @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 deleted file mode 100644 index b750cf6d50ffc0e2d9035249070284e905e8c21b..0000000000000000000000000000000000000000 --- a/franka_gripper/msg/GraspEpsilon.msg +++ /dev/null @@ -1,2 +0,0 @@ -float64 inner # [m] -float64 outer # [m] diff --git a/franka_gripper/src/franka_gripper.cpp b/franka_gripper/src/franka_gripper.cpp index a58493dc880d3e608dc1a50870c44df36e954e2c..5f7ffc66a0fc68e26ce9820bbafc523bcf823e43 100644 --- a/franka_gripper/src/franka_gripper.cpp +++ b/franka_gripper/src/franka_gripper.cpp @@ -29,7 +29,6 @@ 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) { @@ -43,8 +42,7 @@ 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, - grasp_epsilon.inner, grasp_epsilon.outer); + return gripper.grasp(goal->command.position, default_speed, goal->command.max_effort); }; try { @@ -79,8 +77,7 @@ 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, goal->epsilon.inner, - goal->epsilon.outer); + return gripper.grasp(goal->width, goal->speed, goal->force); } } // namespace franka_gripper diff --git a/franka_gripper/src/franka_gripper_node.cpp b/franka_gripper/src/franka_gripper_node.cpp index 8b5b5eaeeab8a2e39c67d0b8e0d2623f5267f66c..31ed7c265f28b01a904d8fb715ad3105dbda6383 100644 --- a/franka_gripper/src/franka_gripper_node.cpp +++ b/franka_gripper/src/franka_gripper_node.cpp @@ -38,7 +38,6 @@ 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; @@ -71,17 +70,6 @@ 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 = @@ -116,8 +104,8 @@ int main(int argc, char** argv) { SimpleActionServer<GripperCommandAction> gripper_command_action_server( node_handle, "gripper_action", - std::bind(&gripperCommandExecuteCallback, std::cref(gripper), default_grasp_epsilon, - default_speed, &gripper_command_action_server, std::placeholders::_1), + std::bind(&gripperCommandExecuteCallback, std::cref(gripper), default_speed, + &gripper_command_action_server, std::placeholders::_1), false); homing_action_server_.start();