From 24057c2abe98a6bc7c6677a3988d48667d4ae56e Mon Sep 17 00:00:00 2001 From: Christoph Krause Date: Fri, 24 Jul 2020 13:27:59 +0000 Subject: [PATCH] changed calculation of feedback position to match encoder values from register gPO --- .../src/robotiq_2f_gripper_action_server.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/robotiq_2f_gripper_action_server/src/robotiq_2f_gripper_action_server.cpp b/robotiq_2f_gripper_action_server/src/robotiq_2f_gripper_action_server.cpp index 4283a419..8130add5 100644 --- a/robotiq_2f_gripper_action_server/src/robotiq_2f_gripper_action_server.cpp +++ b/robotiq_2f_gripper_action_server/src/robotiq_2f_gripper_action_server.cpp @@ -63,7 +63,8 @@ namespace double dist_per_tick = (params.max_gap_ - params.min_gap_) / 255; double eff_per_tick = (params.max_effort_ - params.min_effort_) / 255; - result.position = input.gPO * dist_per_tick + params.min_gap_; + + result.position = params.max_gap_ - input.gPO * dist_per_tick; result.effort = input.gCU * eff_per_tick + params.min_effort_; result.stalled = input.gOBJ == 0x1 || input.gOBJ == 0x2; result.reached_goal = input.gPO == goal_pos;