Grasp method in Kinematic Simulator and IRT Viewer

jsk_kinova_robot/kinovaeus/gen3-interface.l

@@ -30,9 +30,13 @@
      (cons :joint-names (list "joint_1" "joint_2" "joint_3" "joint_4" "joint_5" "joint_6" "joint_7")))))
   (:go-grasp (&key (pos 0.0) (wait t))
     (when (send self :simulation-modep)
-      ;; TODO
-      ;; Move gripper in robot variable like below
-      ;; (send robot :left_inner_finger_joint :joint-angle 50)
+      (let ((pos-deg (rad2deg pos)))
+        (send robot :finger_joint :joint-angle pos-deg)
+        (send robot :left_inner_finger_joint :joint-angle (* -1 pos-deg))
+        (send robot :left_inner_knuckle_joint :joint-angle pos-deg)
+        (send robot :right_inner_finger_joint :joint-angle (* -1 pos-deg))
+        (send robot :right_inner_knuckle_joint :joint-angle pos-deg)
+        (send robot :right_outer_knuckle_joint :joint-angle pos-deg))
       (return-from :go-grasp t))
     (let ((pos-max 0.8) (pos-min 0.0))
       (when (or (< pos pos-min) (> pos pos-max))

jsk_kinova_robot/kinovaeus/gen3-lite-interface.l

@@ -30,11 +30,16 @@
      (cons :joint-names (list "joint_1" "joint_2" "joint_3" "joint_4" "joint_5" "joint_6")))))
   (:go-grasp (&key (pos 0.0) (wait t))
     (when (send self :simulation-modep)
-      ;; TODO
-      ;; Move gripper in robot variable like below
-      ;; (send robot :left_finger_tip_joint :joint-angle -25)
+      (send robot :right_finger_bottom_joint :joint-angle
+            (rad2deg pos))
+      (send robot :right_finger_tip_joint :joint-angle
+            (rad2deg (+ (* -0.676 pos) 0.149)))
+      (send robot :left_finger_bottom_joint :joint-angle
+            (rad2deg (* -1 pos)))
+      (send robot :left_finger_tip_joint :joint-angle
+            (rad2deg (+ (* -0.676 pos) 0.149)))
       (return-from :go-grasp t))
-    (let ((pos-max 0.95) (pos-min -0.1))
+    (let ((pos-max 0.95) (pos-min -0.05))
       (when (or (< pos pos-min) (> pos pos-max))
       (ros::ros-warn (format nil ":pos ~A is out of range." pos))
       (setq pos (max pos-min (min pos pos-max)))))
@@ -47,7 +52,7 @@
       result))
   (:start-grasp
     (&rest args &key &allow-other-keys)
-    (send* self :go-grasp :pos -0.1 args))
+    (send* self :go-grasp :pos -0.05 args))
   (:stop-grasp
     (&rest args &key &allow-other-keys)
     (send* self :go-grasp :pos 0.95 args))

jsk_kinova_robot/kinovaeus/gen3-lite-utils.l

@@ -4,4 +4,17 @@
 
 (defmethod gen3_lite_gen3_lite_2f-robot
   (:arm (&rest args) (send* self :rarm args)) ;; enable to call send *gen3-lite* :arm :angle-vector
+  (:go-grasp (&key (pos 0))
+    (send self :right_finger_bottom_joint :joint-angle
+          (rad2deg pos))
+    (send self :right_finger_tip_joint :joint-angle
+          (rad2deg (+ (* -0.676 pos) 0.149)))
+    (send self :left_finger_bottom_joint :joint-angle
+          (rad2deg (* -1 pos)))
+    (send self :left_finger_tip_joint :joint-angle
+          (rad2deg (+ (* -0.676 pos) 0.149))))
+  (:start-grasp ()
+    (send self :go-grasp :pos -0.05))
+  (:stop-grasp ()
+    (send self :go-grasp :pos 0.95))
   )

jsk_kinova_robot/kinovaeus/gen3-utils.l

@@ -4,4 +4,16 @@
 
 (defmethod gen3_robotiq_2f_85-robot
   (:arm (&rest args) (send* self :rarm args)) ;; enable to call send *gen3* :arm :angle-vector
+  (:go-grasp (&key (pos 0))
+    (let ((pos-deg (rad2deg pos)))
+      (send self :finger_joint :joint-angle pos-deg)
+      (send self :left_inner_finger_joint :joint-angle (* -1 pos-deg))
+      (send self :left_inner_knuckle_joint :joint-angle pos-deg)
+      (send self :right_inner_finger_joint :joint-angle (* -1 pos-deg))
+      (send self :right_inner_knuckle_joint :joint-angle pos-deg)
+      (send self :right_outer_knuckle_joint :joint-angle pos-deg)))
+  (:start-grasp ()
+    (send self :go-grasp :pos 0.8))
+  (:stop-grasp ()
+    (send self :go-grasp :pos 0.0))
   )