Add kinovaeus

jsk_kinova_robot/README.md

@@ -0,0 +1,117 @@
+# jsk_kinova_robot
+
+ROS package for KINOVA Gen3/Gen3 Lite robot.
+
+- KINOVA web page
+  https://www.kinovarobotics.com/en/products/gen3-robot
+- GitHub
+  https://github.com/Kinovarobotics/ros_kortex
+
+
+## How to setup development environment
+
+Use `wstool`, `rosdep` and `catkin` to checkout and compile the source tree.
+
+```bash
+mkdir -p ~/kinova_ws/src
+cd ~/kinova_ws/src
+wstool init
+### TODO: Change this line 708yamaguchi -> jsk-ros-pkg if approved ##
+wstool merge https://raw.githubusercontent.com/708yamaguchi/jsk_robot/kinova-gen3/jsk_kinova_robot/kinova.rosinstall
+wstool update
+cd ../
+source /opt/ros/melodic/setup.bash
+rosdep install -y -r --from-paths src --ignore-src
+catkin build jsk_kinova_robot kinovaeus
+source devel/setup.bash
+```
+
+## KINOVA Setup
+
+TODO:
+
+Setup real kinova robot
+
+MEMO:
+
+https://github.com/jsk-ros-pkg/jsk_robot/blob/dec36f6906b5cfad0a1debb19b9d17b5e7753e75/jsk_denso_robot/README.md
+
+## Start ROS Node
+
+TODO:
+
+Start real kinova robot
+
+```bash
+roslaunch xxx
+```
+
+## Use EusLisp model
+To control the robot form EusLisp. Please start `roseus` and type as follows.
+```
+;; Gen3
+(load "package://kinovaeus/gen3-interface.l")
+(gen3-init)
+;; Gen3 Lite
+(load "package://kinovaeus/gen3-lite-interface.l")
+(gen3-lite-init)
+```
+
+Use `:angle-vector` method to specify the arm joint angle.
+```
+(send *gen3* :angle-vector #f(0.0 15.0 180.0 -130.0 0.0 55.0 90.0))
+```
+
+You can also use `:inverse-kinematics` method to specify the arm pose from target coordinates.
+```
+(send *gen3* :arm :inverse-kinematics (make-coords :pos #f(300 0 200) :rpy (float-vector pi  0 pi)) :debug-view t)
+```
+
+To move the gripper 50 [mm] up, you can use `move-end-pos` method.
+```
+(send *gen3* :arm :move-end-pos #f(0 0 -50))
+```
+
+You can also use move-end-rot method to turn the gripper.
+```
+(send *gen3* :arm :move-end-rot -90 :z)
+```
+
+To control real robot. you can use *ri* object.
+```
+(send *ri* :angle-vector (send *gen3* :angle-vector) 2000)
+```
+2000 indicates we ask robot to move for 2000 [msec]
+
+To obtain current robot pose, use :state :potentio-vector method.
+```
+(send *ri* :state :potentio-vector)
+```
+
+To open and close the gripper. You can use :start-grasp and :stop-grasp.
+```
+(send *ri* :stop-grasp)
+(send *ri* :start-grasp)
+```
+
+## Gazebo simulation
+Kinova Gen3 robot with robotiq 2f 85 gripper.
+```bash
+# Terminal 1
+roslaunch kortex_gazebo spawn_kortex_robot.launch arm:=gen3 gripper:=robotiq_2f_85
+# Terminal 2
+roscd kinovaeus
+roseus gen3-interface.l
+```
+
+Kinova Gen3 Lite robot with lite 2f gripper.
+```bash
+# Terminal 1
+roslaunch kortex_gazebo spawn_kortex_robot.launch arm:=gen3_lite gripper:=gen3_lite_2f
+# Terminal 2
+roscd kinovaeus
+roseus gen3-lite-interface.l
+```
+
+---
+If you have any question, please feel free to file open at https://github.com/jsk-ros-pkg/jsk_robot/issues

jsk_kinova_robot/kinova.rosinstall

@@ -0,0 +1,10 @@
+# TODO
+# Use jsk-ros-pkg/jsk_robot master after approved
+- git:
+    local-name: jsk_robot
+    uri: https://github.com/708yamaguchi/jsk_robot.git
+    version: kinova-gen3
+- git:
+    local-name: ros_kortex
+    uri: https://github.com/Kinovarobotics/ros_kortex
+    version: kinetic-devel

jsk_kinova_robot/kinovaeus/.gitignore

@@ -0,0 +1,4 @@
+gen3.l
+gen3-lite.l
+gen3.urdf
+gen3-lite.urdf

jsk_kinova_robot/kinovaeus/CMakeLists.txt

@@ -0,0 +1,68 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(kinovaeus)
+
+find_package(catkin REQUIRED COMPONENTS
+  roseus
+  euscollada
+  kortex_description
+)
+
+catkin_package()
+
+if(NOT kortex_description_FOUND)
+  message(WARNING "kortex_description is not found, so skip generating eus models.")
+  message(WARNING "Install kortex_description from https://github.com/Kinovarobotics/ros_kortex.git")
+  return()
+endif()
+
+
+###########
+## Build ##
+###########
+if(EXISTS ${kortex_description_SOURCE_PREFIX}/robots)
+  set(_kinova_urdf ${kortex_description_SOURCE_PREFIX}/robots)
+else()
+  set(_kinova_urdf ${kortex_description_PREFIX}/share/kortex_description/robots)
+endif()
+set(_collada2eus ${euscollada_PREFIX}/lib/euscollada/collada2eus)
+
+message("kinova_urdf: ${_kinova_urdf}")
+message("collada2eus: ${_collada2eus}")
+
+# Gen3 robot with robotiq gripper
+# xacro command: https://github.com/Kinovarobotics/ros_kortex/tree/kinetic-devel/kortex_description
+add_custom_command(OUTPUT ${PROJECT_BINARY_DIR}/gen3.urdf
+  COMMAND xacro ${_kinova_urdf}/gen3_robotiq_2f_85.xacro sim:=false > ${PROJECT_BINARY_DIR}/gen3.urdf
+  DEPENDS ${_kinova_urdf}/gen3_robotiq_2f_85.xacro)
+add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/gen3.l
+  COMMAND echo "${_collada2eus} ${PROJECT_BINARY_DIR}/gen3.urdf gen3.l"
+  COMMAND ${_collada2eus} ${PROJECT_BINARY_DIR}/gen3.urdf ${PROJECT_SOURCE_DIR}/gen3.yaml ${PROJECT_SOURCE_DIR}/gen3.l
+  DEPENDS ${PROJECT_BINARY_DIR}/gen3.urdf ${PROJECT_SOURCE_DIR}/gen3.yaml ${_collada2eus})
+add_custom_target(compile_gen3 ALL DEPENDS ${PROJECT_SOURCE_DIR}/gen3.l)
+
+# Gen3 Lite robot
+# xacro command: https://github.com/Kinovarobotics/ros_kortex/tree/kinetic-devel/kortex_description
+add_custom_command(OUTPUT ${PROJECT_BINARY_DIR}/gen3-lite.urdf
+  COMMAND xacro ${_kinova_urdf}/gen3_lite_gen3_lite_2f.xacro sim:=false > ${PROJECT_BINARY_DIR}/gen3-lite.urdf
+  DEPENDS ${_kinova_urdf}/gen3_lite_gen3_lite_2f.xacro)
+add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/gen3-lite.l
+  COMMAND echo "${_collada2eus} ${PROJECT_BINARY_DIR}/gen3-lite.urdf gen3-lite.l"
+  COMMAND ${_collada2eus} ${PROJECT_BINARY_DIR}/gen3-lite.urdf ${PROJECT_SOURCE_DIR}/gen3-lite.yaml ${PROJECT_SOURCE_DIR}/gen3-lite.l
+  DEPENDS ${PROJECT_BINARY_DIR}/gen3-lite.urdf ${PROJECT_SOURCE_DIR}/gen3-lite.yaml ${_collada2eus})
+add_custom_target(compile_gen3_lite ALL DEPENDS ${PROJECT_SOURCE_DIR}/gen3-lite.l)
+
+install(DIRECTORY test
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS)
+
+install(FILES gen3.l gen3-lite.l gen3-interface.l gen3-lite-interface.l gen3-utils.l gen3-lite-utils.l gen3.yaml gen3-lite.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(catkin REQUIRED COMPONENTS rostest)
+  # Test with Kinematic Simulator
+  add_rostest(test/test-gen3.test)
+  add_rostest(test/test-gen3-lite.test)
+  # TODO: Test with Gazebo
+  # add_rostest(test/test-gen3-gazebo.test)
+  # add_rostest(test/test-gen3-lite-gazebo.test)
+endif()

jsk_kinova_robot/kinovaeus/gen3-interface.l

@@ -0,0 +1,65 @@
+(require "package://kinovaeus/gen3-utils.l")
+(require "package://pr2eus/robot-interface.l")
+
+;; TODO
+;; Enable to use moveit
+
+(defclass gen3-interface
+  :super robot-interface
+  :slots (gripper-action)
+  )
+
+(defmethod gen3-interface
+  (:init
+   (&rest args)
+   (send-super* :init :robot gen3_robotiq_2f_85-robot :joint-states-topic "/my_gen3/joint_states" :groupname "gen3_interface" args)
+   (send self :add-controller :rarm-controller)
+   (setq gripper-action
+         (instance ros::simple-action-client :init
+                   "/my_gen3/robotiq_2f_85_gripper_controller/gripper_cmd"
+                   control_msgs::GripperCommandAction
+                   :groupname groupname))
+   )
+  (:default-controller () (send self :rarm-controller))
+  (:rarm-controller ()
+   (list
+    (list
+     (cons :controller-action "my_gen3/gen3_joint_trajectory_controller/follow_joint_trajectory")
+     (cons :controller-state "my_gen3/gen3_joint_trajectory_controller/state")
+     (cons :action-type control_msgs::FollowJointTrajectoryAction)
+     (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)
+      (return-from :go-grasp t))
+    (let ((pos-max 0.8) (pos-min 0.0))
+      (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)))))
+    (let (goal result)
+      (setq goal (instance control_msgs::GripperCommandActionGoal :init))
+      (send goal :goal :command :position pos)
+      (send gripper-action :send-goal goal)
+      (when wait (send gripper-action :wait-for-result))
+      (setq result (send gripper-action :get-result))
+      result))
+  (:start-grasp
+    (&rest args &key &allow-other-keys)
+    (send* self :go-grasp :pos 0.8 args))
+  (:stop-grasp
+    (&rest args &key &allow-other-keys)
+    (send* self :go-grasp :pos 0.0 args))
+  )
+
+(defun gen3-init (&optional (create-viewer))
+  (unless (boundp '*gen3*) (gen3) (send *gen3* :reset-pose))
+  (unless (ros::ok) (ros::roseus "gen3_eus_interface"))
+  (unless (boundp '*ri*) (setq *ri* (instance gen3-interface :init)))
+
+  (ros::spin-once)
+  (send *ri* :spin-once)
+
+  (when create-viewer (objects (list *gen3*)))
+  )

jsk_kinova_robot/kinovaeus/gen3-lite-interface.l

@@ -0,0 +1,65 @@
+(require "package://kinovaeus/gen3-lite-utils.l")
+(require "package://pr2eus/robot-interface.l")
+
+;; TODO
+;; Enable to use moveit
+
+(defclass gen3-lite-interface
+  :super robot-interface
+  :slots (gripper-action)
+  )
+
+(defmethod gen3-lite-interface
+  (:init
+   (&rest args)
+   (send-super* :init :robot gen3_lite_gen3_lite_2f-robot :joint-states-topic "/my_gen3_lite/joint_states" :groupname "gen3_lite_interface" args)
+   (send self :add-controller :rarm-controller)
+   (setq gripper-action
+         (instance ros::simple-action-client :init
+                   "/my_gen3_lite/gen3_lite_2f_gripper_controller/gripper_cmd"
+                   control_msgs::GripperCommandAction
+                   :groupname groupname))
+   )
+  (:default-controller () (send self :rarm-controller))
+  (:rarm-controller ()
+   (list
+    (list
+     (cons :controller-action "my_gen3_lite/gen3_lite_joint_trajectory_controller/follow_joint_trajectory")
+     (cons :controller-state "my_gen3_lite/gen3_lite_joint_trajectory_controller/state")
+     (cons :action-type control_msgs::FollowJointTrajectoryAction)
+     (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)
+      (return-from :go-grasp t))
+    (let ((pos-max 0.95) (pos-min -0.1))
+      (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)))))
+    (let (goal result)
+      (setq goal (instance control_msgs::GripperCommandActionGoal :init))
+      (send goal :goal :command :position pos)
+      (send gripper-action :send-goal goal)
+      (when wait (send gripper-action :wait-for-result))
+      (setq result (send gripper-action :get-result))
+      result))
+  (:start-grasp
+    (&rest args &key &allow-other-keys)
+    (send* self :go-grasp :pos -0.1 args))
+  (:stop-grasp
+    (&rest args &key &allow-other-keys)
+    (send* self :go-grasp :pos 0.95 args))
+  )
+
+(defun gen3-lite-init (&optional (create-viewer))
+  (unless (boundp '*gen3-lite*) (gen3-lite) (send *gen3-lite* :reset-pose))
+  (unless (ros::ok) (ros::roseus "gen3_lite_eus_interface"))
+  (unless (boundp '*ri*) (setq *ri* (instance gen3-lite-interface :init)))
+
+  (ros::spin-once)
+  (send *ri* :spin-once)
+
+  (when create-viewer (objects (list *gen3-lite*)))
+  )

jsk_kinova_robot/kinovaeus/gen3-lite-utils.l

@@ -0,0 +1,7 @@
+(require :gen3 "package://kinovaeus/gen3-lite.l")
+
+(defun gen3-lite () (setq *gen3-lite* (instance gen3_lite_gen3_lite_2f-robot :init)))
+
+(defmethod gen3_lite_gen3_lite_2f-robot
+  (:arm (&rest args) (send* self :rarm args)) ;; enable to call send *gen3-lite* :arm :angle-vector
+  )

jsk_kinova_robot/kinovaeus/gen3-lite.yaml

@@ -0,0 +1,20 @@
+##
+## - collada_joint_name : euslisp_joint_name (start with :)
+##
+
+rarm:
+  - joint_1 : rarm-shoulder-r
+  - joint_2 : rarm-shoulder-p
+  - joint_3 : rarm-elbow-r
+  - joint_4 : rarm-elbow-p
+  - joint_5 : rarm-wrist-r
+  - joint_6 : rarm-wrist-p
+
+angle-vector:
+  init-pose : [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+  reset-pose: [0.0, -16.0, 75.0, 0.0, -60.0, 0.0]
+
+rarm-end-coords:
+  parent : end_effector_link
+  translate : [0, 0, 0]
+  rotate : [0, 0, 1, 0]

jsk_kinova_robot/kinovaeus/gen3-utils.l

@@ -0,0 +1,7 @@
+(require :gen3 "package://kinovaeus/gen3.l")
+
+(defun gen3 () (setq *gen3* (instance gen3_robotiq_2f_85-robot :init)))
+
+(defmethod gen3_robotiq_2f_85-robot
+  (:arm (&rest args) (send* self :rarm args)) ;; enable to call send *gen3* :arm :angle-vector
+  )

jsk_kinova_robot/kinovaeus/gen3.yaml

@@ -0,0 +1,21 @@
+##
+## - collada_joint_name : euslisp_joint_name (start with :)
+##
+
+rarm:
+  - joint_1 : rarm-shoulder-r
+  - joint_2 : rarm-shoulder-p
+  - joint_3 : rarm-elbow-r
+  - joint_4 : rarm-elbow-p
+  - joint_5 : rarm-wrist-r
+  - joint_6 : rarm-wrist-p
+  - joint_7 : rarm-wrist-y
+
+angle-vector:
+  init-pose : [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+  reset-pose : [0.0, 15.0, 180.0, -130.0, 0.0, 55.0, 90.0]
+
+rarm-end-coords:
+  parent : end_effector_link
+  translate : [0, 0, 0]
+  rotate : [0, 0, 1, 0]