diff --git a/jsk_kinova_robot/kinovaeus/kinova-interface.l b/jsk_kinova_robot/kinovaeus/kinova-interface.l index 1cf5e1f419..d6e352de71 100644 --- a/jsk_kinova_robot/kinovaeus/kinova-interface.l +++ b/jsk_kinova_robot/kinovaeus/kinova-interface.l @@ -168,13 +168,13 @@ (when (send self :simulation-modep) ;; mimic joint ;; https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_macro.xacro - (send robot :right_finger_bottom_joint :joint-angle + (send robot :kinova_right_finger_bottom_joint :joint-angle (rad2deg pos)) - (send robot :right_finger_tip_joint :joint-angle + (send robot :kinova_right_finger_tip_joint :joint-angle (rad2deg (+ (* -0.676 pos) 0.149))) - (send robot :left_finger_bottom_joint :joint-angle + (send robot :kinova_left_finger_bottom_joint :joint-angle (rad2deg (* -1 pos))) - (send robot :left_finger_tip_joint :joint-angle + (send robot :kinova_left_finger_tip_joint :joint-angle (rad2deg (+ (* -0.676 pos) 0.149))) (return-from :go-grasp-lite t)) (let ((pos-max 0.95) (pos-min -0.05)) diff --git a/jsk_spotkinova_robot/spotkinovaeus/spotkinova.l b/jsk_spotkinova_robot/spotkinovaeus/spotkinova.l index 2928ff68d6..1ce3482f54 100644 --- a/jsk_spotkinova_robot/spotkinovaeus/spotkinova.l +++ b/jsk_spotkinova_robot/spotkinovaeus/spotkinova.l @@ -244,13 +244,13 @@ Example: (:go-grasp (&key (pos 0)) ;; mimic joint ;; https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_macro.xacro - (send self :right_finger_bottom_joint :joint-angle + (send self :kinova_right_finger_bottom_joint :joint-angle (rad2deg pos)) - (send self :right_finger_tip_joint :joint-angle + (send self :kinova_right_finger_tip_joint :joint-angle (rad2deg (+ (* -0.676 pos) 0.149))) - (send self :left_finger_bottom_joint :joint-angle + (send self :kinova_left_finger_bottom_joint :joint-angle (rad2deg (* -1 pos))) - (send self :left_finger_tip_joint :joint-angle + (send self :kinova_left_finger_tip_joint :joint-angle (rad2deg (+ (* -0.676 pos) 0.149)))) (:start-grasp () (send self :go-grasp :pos -0.05))