From d638cb7ad85fbeeee05ce76b17103f9a7daea5eb Mon Sep 17 00:00:00 2001 From: Simon Gabl <simon.gabl@franka.de> Date: Tue, 12 Sep 2017 19:35:04 +0200 Subject: [PATCH] Removed newton_to_m_ampere_factor, changed gripper grasp parameter from mA to N. --- franka_gripper/action/Grasp.action | 2 +- franka_gripper/config/gripper_node_config.yaml | 3 +-- franka_gripper/include/franka_gripper/franka_gripper.h | 2 -- franka_gripper/src/franka_gripper.cpp | 6 ++---- franka_gripper/src/franka_gripper_node.cpp | 8 +------- 5 files changed, 5 insertions(+), 16 deletions(-) diff --git a/franka_gripper/action/Grasp.action b/franka_gripper/action/Grasp.action index 3625ca6..4fa4875 100644 --- a/franka_gripper/action/Grasp.action +++ b/franka_gripper/action/Grasp.action @@ -1,6 +1,6 @@ float64 width # [m] float64 speed # [m/s] -float64 max_current # [mA] +float64 max_force # [N] --- bool success string error diff --git a/franka_gripper/config/gripper_node_config.yaml b/franka_gripper/config/gripper_node_config.yaml index bb10ea4..eb375d8 100644 --- a/franka_gripper/config/gripper_node_config.yaml +++ b/franka_gripper/config/gripper_node_config.yaml @@ -3,5 +3,4 @@ joint_names: - franka_emika_finger_joint1 - franka_emika_finger_joint2 width_tolerance: 0.01 # [m] -default_speed: 0.1 # [m/s] -newton_to_m_ampere_factor: 14.9 # [mA/N] +default_speed: 0.1 # [m/s] \ 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 4455fd2..ecae821 100644 --- a/franka_gripper/include/franka_gripper/franka_gripper.h +++ b/franka_gripper/include/franka_gripper/franka_gripper.h @@ -31,14 +31,12 @@ 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] newton_to_m_ampere_factor The mapping factor from Newton to milliampere * @param[in] action_server A pointer to a gripper action server * @param[in] goal A gripper action goal */ void gripperCommandExecuteCallback( const franka::Gripper& gripper, double default_speed, - double newton_to_m_ampere_factor, actionlib::SimpleActionServer<control_msgs::GripperCommandAction>* action_server, const control_msgs::GripperCommandGoalConstPtr& goal); diff --git a/franka_gripper/src/franka_gripper.cpp b/franka_gripper/src/franka_gripper.cpp index 53ce94e..7596ea2 100644 --- a/franka_gripper/src/franka_gripper.cpp +++ b/franka_gripper/src/franka_gripper.cpp @@ -28,7 +28,6 @@ bool updateGripperState(const franka::Gripper& gripper, franka::GripperState* st void gripperCommandExecuteCallback( const franka::Gripper& gripper, double default_speed, - double newton_to_m_ampere_factor, actionlib::SimpleActionServer<control_msgs::GripperCommandAction>* action_server, const control_msgs::GripperCommandGoalConstPtr& goal) { std::function<bool()> gripper_command_handler = [=, &gripper]() { @@ -41,8 +40,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 * newton_to_m_ampere_factor); + return gripper.grasp(goal->command.position, default_speed, goal->command.max_effort); }; try { @@ -77,7 +75,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->max_current); + return gripper.grasp(goal->width, goal->speed, goal->max_force); } } // namespace franka_gripper diff --git a/franka_gripper/src/franka_gripper_node.cpp b/franka_gripper/src/franka_gripper_node.cpp index 9ad8f43..407fc52 100644 --- a/franka_gripper/src/franka_gripper_node.cpp +++ b/franka_gripper/src/franka_gripper_node.cpp @@ -73,12 +73,6 @@ int main(int argc, char** argv) { ROS_INFO_STREAM("franka_gripper_node: Found default_speed" << default_speed); } - double newton_to_m_ampere_factor(14.9); - if (node_handle.getParam("newton_to_m_ampere_factor", newton_to_m_ampere_factor)) { - ROS_INFO_STREAM("franka_gripper_node: Found newton_to_m_ampere_factor" - << newton_to_m_ampere_factor); - } - franka::Gripper gripper(robot_ip); std::function<bool(const HomingGoalConstPtr&)> homing_handler = @@ -114,7 +108,7 @@ int main(int argc, char** argv) { SimpleActionServer<GripperCommandAction> gripper_command_action_server( node_handle, "gripper_action", std::bind(&gripperCommandExecuteCallback, std::cref(gripper), default_speed, - newton_to_m_ampere_factor, &gripper_command_action_server, std::placeholders::_1), + &gripper_command_action_server, std::placeholders::_1), false); homing_action_server_.start(); -- GitLab