From 7c455d8926ad6e6b593d56acb9bded92f2a21534 Mon Sep 17 00:00:00 2001
From: Naoya Yamaguchi <708yamaguchi@gmail.com>
Date: Sat, 2 Jan 2021 23:54:05 +0900
Subject: [PATCH] Add kinovaeus
---
jsk_kinova_robot/README.md | 117 ++++++++++++++++++
jsk_kinova_robot/kinova.rosinstall | 10 ++
jsk_kinova_robot/kinovaeus/.gitignore | 4 +
jsk_kinova_robot/kinovaeus/CMakeLists.txt | 68 ++++++++++
jsk_kinova_robot/kinovaeus/gen3-interface.l | 65 ++++++++++
.../kinovaeus/gen3-lite-interface.l | 65 ++++++++++
jsk_kinova_robot/kinovaeus/gen3-lite-utils.l | 7 ++
jsk_kinova_robot/kinovaeus/gen3-lite.yaml | 20 +++
jsk_kinova_robot/kinovaeus/gen3-utils.l | 7 ++
jsk_kinova_robot/kinovaeus/gen3.yaml | 21 ++++
jsk_kinova_robot/kinovaeus/package.xml | 26 ++++
.../kinovaeus/test/test-gen3-gazebo.test | 10 ++
.../kinovaeus/test/test-gen3-lite-gazebo.test | 10 ++
.../kinovaeus/test/test-gen3-lite.l | 28 +++++
.../kinovaeus/test/test-gen3-lite.test | 3 +
jsk_kinova_robot/kinovaeus/test/test-gen3.l | 28 +++++
.../kinovaeus/test/test-gen3.test | 3 +
17 files changed, 492 insertions(+)
create mode 100644 jsk_kinova_robot/README.md
create mode 100644 jsk_kinova_robot/kinova.rosinstall
create mode 100644 jsk_kinova_robot/kinovaeus/.gitignore
create mode 100644 jsk_kinova_robot/kinovaeus/CMakeLists.txt
create mode 100644 jsk_kinova_robot/kinovaeus/gen3-interface.l
create mode 100644 jsk_kinova_robot/kinovaeus/gen3-lite-interface.l
create mode 100644 jsk_kinova_robot/kinovaeus/gen3-lite-utils.l
create mode 100644 jsk_kinova_robot/kinovaeus/gen3-lite.yaml
create mode 100644 jsk_kinova_robot/kinovaeus/gen3-utils.l
create mode 100644 jsk_kinova_robot/kinovaeus/gen3.yaml
create mode 100644 jsk_kinova_robot/kinovaeus/package.xml
create mode 100644 jsk_kinova_robot/kinovaeus/test/test-gen3-gazebo.test
create mode 100644 jsk_kinova_robot/kinovaeus/test/test-gen3-lite-gazebo.test
create mode 100755 jsk_kinova_robot/kinovaeus/test/test-gen3-lite.l
create mode 100644 jsk_kinova_robot/kinovaeus/test/test-gen3-lite.test
create mode 100755 jsk_kinova_robot/kinovaeus/test/test-gen3.l
create mode 100644 jsk_kinova_robot/kinovaeus/test/test-gen3.test
diff --git a/jsk_kinova_robot/README.md b/jsk_kinova_robot/README.md
new file mode 100644
index 00000000000..336bbdbcaed
--- /dev/null
+++ b/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
diff --git a/jsk_kinova_robot/kinova.rosinstall b/jsk_kinova_robot/kinova.rosinstall
new file mode 100644
index 00000000000..57485d3324b
--- /dev/null
+++ b/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
diff --git a/jsk_kinova_robot/kinovaeus/.gitignore b/jsk_kinova_robot/kinovaeus/.gitignore
new file mode 100644
index 00000000000..8d29999b25b
--- /dev/null
+++ b/jsk_kinova_robot/kinovaeus/.gitignore
@@ -0,0 +1,4 @@
+gen3.l
+gen3-lite.l
+gen3.urdf
+gen3-lite.urdf
diff --git a/jsk_kinova_robot/kinovaeus/CMakeLists.txt b/jsk_kinova_robot/kinovaeus/CMakeLists.txt
new file mode 100644
index 00000000000..7fa7357b939
--- /dev/null
+++ b/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()
diff --git a/jsk_kinova_robot/kinovaeus/gen3-interface.l b/jsk_kinova_robot/kinovaeus/gen3-interface.l
new file mode 100644
index 00000000000..e2aff5f6a6a
--- /dev/null
+++ b/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*)))
+ )
diff --git a/jsk_kinova_robot/kinovaeus/gen3-lite-interface.l b/jsk_kinova_robot/kinovaeus/gen3-lite-interface.l
new file mode 100644
index 00000000000..1363dd2fd4a
--- /dev/null
+++ b/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*)))
+ )
diff --git a/jsk_kinova_robot/kinovaeus/gen3-lite-utils.l b/jsk_kinova_robot/kinovaeus/gen3-lite-utils.l
new file mode 100644
index 00000000000..e4a60f132b0
--- /dev/null
+++ b/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
+ )
diff --git a/jsk_kinova_robot/kinovaeus/gen3-lite.yaml b/jsk_kinova_robot/kinovaeus/gen3-lite.yaml
new file mode 100644
index 00000000000..9e2f76a4771
--- /dev/null
+++ b/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]
diff --git a/jsk_kinova_robot/kinovaeus/gen3-utils.l b/jsk_kinova_robot/kinovaeus/gen3-utils.l
new file mode 100644
index 00000000000..e0dc6fe195d
--- /dev/null
+++ b/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
+ )
diff --git a/jsk_kinova_robot/kinovaeus/gen3.yaml b/jsk_kinova_robot/kinovaeus/gen3.yaml
new file mode 100644
index 00000000000..8aeec5b5cbc
--- /dev/null
+++ b/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]
diff --git a/jsk_kinova_robot/kinovaeus/package.xml b/jsk_kinova_robot/kinovaeus/package.xml
new file mode 100644
index 00000000000..fb145e6bf13
--- /dev/null
+++ b/jsk_kinova_robot/kinovaeus/package.xml
@@ -0,0 +1,26 @@
+
+
+ kinovaeus
+ 1.0.0
+ The kinova package
+
+ Naoya Yamaguchi
+
+ BSD
+ Naoya Yamaguchi
+
+ catkin
+
+ collada_urdf
+ euscollada
+ rostest
+ roseus
+ pr2eus
+ kortex_description
+
+ roseus
+ pr2eus
+
+
+
+
diff --git a/jsk_kinova_robot/kinovaeus/test/test-gen3-gazebo.test b/jsk_kinova_robot/kinovaeus/test/test-gen3-gazebo.test
new file mode 100644
index 00000000000..0603c51a26d
--- /dev/null
+++ b/jsk_kinova_robot/kinovaeus/test/test-gen3-gazebo.test
@@ -0,0 +1,10 @@
+
+
+
+
+
+
+
+
+
+
diff --git a/jsk_kinova_robot/kinovaeus/test/test-gen3-lite-gazebo.test b/jsk_kinova_robot/kinovaeus/test/test-gen3-lite-gazebo.test
new file mode 100644
index 00000000000..4c2e69a5c78
--- /dev/null
+++ b/jsk_kinova_robot/kinovaeus/test/test-gen3-lite-gazebo.test
@@ -0,0 +1,10 @@
+
+
+
+
+
+
+
+
+
+
diff --git a/jsk_kinova_robot/kinovaeus/test/test-gen3-lite.l b/jsk_kinova_robot/kinovaeus/test/test-gen3-lite.l
new file mode 100755
index 00000000000..43c008260f1
--- /dev/null
+++ b/jsk_kinova_robot/kinovaeus/test/test-gen3-lite.l
@@ -0,0 +1,28 @@
+#!/usr/bin/env roseus
+(require :unittest "lib/llib/unittest.l")
+(require "package://kinovaeus/gen3-lite-utils.l")
+(require "package://kinovaeus/gen3-lite-interface.l")
+
+(init-unit-test)
+
+(gen3-lite-init)
+
+(deftest test-gen3-lite-move
+
+ (send *gen3-lite* :reset-pose)
+ (assert (send *ri* :angle-vector (send *gen3-lite* :angle-vector)))
+
+ (send *ri* :start-grasp :wait t)
+
+ (send *gen3-lite* :angle-vector #f(90.0 0.0 25.0 0.0 0.0 0.0 0.0))
+ (assert (send *ri* :angle-vector (send *gen3-lite* :angle-vector)))
+
+ (send *gen3-lite* :arm :inverse-kinematics (make-coords :pos #f(300 0 200) :rpy (float-vector pi 0 pi)) :debug-view t)
+ (assert (send *ri* :angle-vector (send *gen3-lite* :angle-vector)))
+
+ (send *ri* :stop-grasp :wait t)
+ )
+
+
+(run-all-tests)
+(exit)
diff --git a/jsk_kinova_robot/kinovaeus/test/test-gen3-lite.test b/jsk_kinova_robot/kinovaeus/test/test-gen3-lite.test
new file mode 100644
index 00000000000..a46d1ef3a1c
--- /dev/null
+++ b/jsk_kinova_robot/kinovaeus/test/test-gen3-lite.test
@@ -0,0 +1,3 @@
+
+
+
diff --git a/jsk_kinova_robot/kinovaeus/test/test-gen3.l b/jsk_kinova_robot/kinovaeus/test/test-gen3.l
new file mode 100755
index 00000000000..170b35d175c
--- /dev/null
+++ b/jsk_kinova_robot/kinovaeus/test/test-gen3.l
@@ -0,0 +1,28 @@
+#!/usr/bin/env roseus
+(require :unittest "lib/llib/unittest.l")
+(require "package://kinovaeus/gen3-utils.l")
+(require "package://kinovaeus/gen3-interface.l")
+
+(init-unit-test)
+
+(gen3-init)
+
+(deftest test-gen3-move
+
+ (send *gen3* :reset-pose)
+ (assert (send *ri* :angle-vector (send *gen3* :angle-vector)))
+
+ (send *ri* :start-grasp :wait t)
+
+ (send *gen3* :angle-vector #f(90.0 0.0 25.0 0.0 0.0 0.0 0.0))
+ (assert (send *ri* :angle-vector (send *gen3* :angle-vector)))
+
+ (send *gen3* :arm :inverse-kinematics (make-coords :pos #f(300 0 200) :rpy (float-vector pi 0 pi)) :debug-view t)
+ (assert (send *ri* :angle-vector (send *gen3* :angle-vector)))
+
+ (send *ri* :stop-grasp :wait t)
+ )
+
+
+(run-all-tests)
+(exit)
diff --git a/jsk_kinova_robot/kinovaeus/test/test-gen3.test b/jsk_kinova_robot/kinovaeus/test/test-gen3.test
new file mode 100644
index 00000000000..3d5ffc38a6d
--- /dev/null
+++ b/jsk_kinova_robot/kinovaeus/test/test-gen3.test
@@ -0,0 +1,3 @@
+
+
+