From c7cf7b22976840ab6d5a604fa5efa8642fff0d35 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Sat, 2 Jan 2021 23:54:05 +0900 Subject: [PATCH 01/41] 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 0000000000..336bbdbcae --- /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 0000000000..57485d3324 --- /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 0000000000..8d29999b25 --- /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 0000000000..7fa7357b93 --- /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 0000000000..e2aff5f6a6 --- /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 0000000000..1363dd2fd4 --- /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 0000000000..e4a60f132b --- /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 0000000000..9e2f76a477 --- /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 0000000000..e0dc6fe195 --- /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 0000000000..8aeec5b5cb --- /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 0000000000..fb145e6bf1 --- /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 0000000000..0603c51a26 --- /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 0000000000..4c2e69a5c7 --- /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 0000000000..43c008260f --- /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 0000000000..a46d1ef3a1 --- /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 0000000000..170b35d175 --- /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 0000000000..3d5ffc38a6 --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/test/test-gen3.test @@ -0,0 +1,3 @@ + + + From 4d384dbc976ba3ef51e117a598897052c346ec55 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Sun, 3 Jan 2021 12:07:49 +0900 Subject: [PATCH 02/41] Add test depend --- jsk_kinova_robot/README.md | 3 ++- jsk_kinova_robot/kinovaeus/package.xml | 4 ++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/jsk_kinova_robot/README.md b/jsk_kinova_robot/README.md index 336bbdbcae..6636b68aa6 100644 --- a/jsk_kinova_robot/README.md +++ b/jsk_kinova_robot/README.md @@ -22,7 +22,8 @@ wstool update cd ../ source /opt/ros/melodic/setup.bash rosdep install -y -r --from-paths src --ignore-src -catkin build jsk_kinova_robot kinovaeus +# TODO: add catkin build jsk_kinova_startup +catkin build kinovaeus source devel/setup.bash ``` diff --git a/jsk_kinova_robot/kinovaeus/package.xml b/jsk_kinova_robot/kinovaeus/package.xml index fb145e6bf1..761b1c3ba5 100644 --- a/jsk_kinova_robot/kinovaeus/package.xml +++ b/jsk_kinova_robot/kinovaeus/package.xml @@ -21,6 +21,10 @@ roseus pr2eus + kortex_gazebo + gen3_robotiq_2f_85_move_it_config + gen3_lite_gen3_lite_2f_move_it_config + From 22ac6ccb7cfcb8cb4283dc842423bb85ea97f008 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Sun, 3 Jan 2021 12:23:00 +0900 Subject: [PATCH 03/41] Grasp method in Kinematic Simulator and IRT Viewer --- jsk_kinova_robot/kinovaeus/gen3-interface.l | 10 +++++++--- jsk_kinova_robot/kinovaeus/gen3-lite-interface.l | 15 ++++++++++----- jsk_kinova_robot/kinovaeus/gen3-lite-utils.l | 13 +++++++++++++ jsk_kinova_robot/kinovaeus/gen3-utils.l | 12 ++++++++++++ 4 files changed, 42 insertions(+), 8 deletions(-) diff --git a/jsk_kinova_robot/kinovaeus/gen3-interface.l b/jsk_kinova_robot/kinovaeus/gen3-interface.l index e2aff5f6a6..5e4a463ba2 100644 --- a/jsk_kinova_robot/kinovaeus/gen3-interface.l +++ b/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)) diff --git a/jsk_kinova_robot/kinovaeus/gen3-lite-interface.l b/jsk_kinova_robot/kinovaeus/gen3-lite-interface.l index 1363dd2fd4..1a66196c15 100644 --- a/jsk_kinova_robot/kinovaeus/gen3-lite-interface.l +++ b/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)) diff --git a/jsk_kinova_robot/kinovaeus/gen3-lite-utils.l b/jsk_kinova_robot/kinovaeus/gen3-lite-utils.l index e4a60f132b..30b7045132 100644 --- a/jsk_kinova_robot/kinovaeus/gen3-lite-utils.l +++ b/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)) ) diff --git a/jsk_kinova_robot/kinovaeus/gen3-utils.l b/jsk_kinova_robot/kinovaeus/gen3-utils.l index e0dc6fe195..c17d5c33d1 100644 --- a/jsk_kinova_robot/kinovaeus/gen3-utils.l +++ b/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)) ) From f7f76973bb6170613cfda46d52920b2832dd9dd0 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Sun, 3 Jan 2021 16:11:38 +0900 Subject: [PATCH 04/41] Enable to use MoveIt! --- jsk_kinova_robot/kinovaeus/gen3-interface.l | 135 +++++++++++++++++- .../kinovaeus/gen3-lite-interface.l | 135 +++++++++++++++++- jsk_kinova_robot/kinovaeus/package.xml | 2 + 3 files changed, 258 insertions(+), 14 deletions(-) diff --git a/jsk_kinova_robot/kinovaeus/gen3-interface.l b/jsk_kinova_robot/kinovaeus/gen3-interface.l index 5e4a463ba2..74a6a9c5d0 100644 --- a/jsk_kinova_robot/kinovaeus/gen3-interface.l +++ b/jsk_kinova_robot/kinovaeus/gen3-interface.l @@ -1,27 +1,27 @@ (require "package://kinovaeus/gen3-utils.l") (require "package://pr2eus/robot-interface.l") - -;; TODO -;; Enable to use moveit +(require "package://pr2eus_moveit/euslisp/robot-moveit.l") (defclass gen3-interface :super robot-interface - :slots (gripper-action) + :slots (gripper-action moveit-robot) ) (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) + (send self :add-controller :arm-controller) (setq gripper-action (instance ros::simple-action-client :init "/my_gen3/robotiq_2f_85_gripper_controller/gripper_cmd" control_msgs::GripperCommandAction :groupname groupname)) + (setq moveit-robot (instance gen3_robotiq_2f_85-robot :init)) + (send self :set-moveit-environment (instance gen3-moveit-environment :init :robot moveit-robot)) ) - (:default-controller () (send self :rarm-controller)) - (:rarm-controller () + (:default-controller () (send self :arm-controller)) + (:arm-controller () (list (list (cons :controller-action "my_gen3/gen3_joint_trajectory_controller/follow_joint_trajectory") @@ -55,11 +55,110 @@ (:stop-grasp (&rest args &key &allow-other-keys) (send* self :go-grasp :pos 0.0 args)) + ;; :angle-vector-xxx and :check-continuous-joint-move-over-180 + ;; are mainly copied from package://fetcheus/fetch-interface.l + (:check-continuous-joint-move-over-180 ;; can be removed if http//github.com/jsk-ros-pkg/jsk_pr2eus/pull/322 merged + (diff-av) + (let ((i 0) add-new-trajectory-point) + (dolist (j (send robot :joint-list)) + ;; for continuous rotational joint + (when (and (> (- (send j :max-angle) (send j :min-angle)) 360) + (> (abs (elt diff-av i)) 180)) + (ros::ros-warn "continuous joint (~A) moves ~A degree, commanded joint differs from original trajectory to avoid unintentional 360 rotation" (send j :name) (elt diff-av i)) + (setq add-new-trajectory-point t)) + (incf i (send j :joint-dof))) + add-new-trajectory-point)) + (:angle-vector-raw (av &optional (tm 3000) &rest args) + (let* ((prev-av (send self :state :potentio-vector :wait-until-update t)) + (diff-av (v- av prev-av))) + (when (send self :check-continuous-joint-move-over-180 diff-av) + (let* (avs (minjerk (instance minjerk-interpolator :init)) + (scale-av (send self :sub-angle-vector av prev-av)) + dist div) + (setq dist (abs (geo::find-extream (coerce diff-av cons) #'abs #'>=))) + (setq div (round (/ dist 120.0))) + (send minjerk :reset + :position-list (list prev-av (v+ prev-av scale-av)) + :time-list (list tm)) + (send minjerk :start-interpolation) + (send minjerk :pass-time (/ tm div)) + (dotimes (i div) + (setq avs (append avs (list (send minjerk :pass-time (/ tm div)))))) + (send* self :angle-vector-sequence-raw avs (make-list div :initial-element (/ tm div)) args) + (return-from :angle-vector-raw (car (last avs))))) + (send-super* :angle-vector av tm args))) + (:angle-vector-sequence-raw (&rest args) (send-super* :angle-vector-sequence args)) + (:angle-vector + (av &optional (tm 3000) &rest args) ;; (ctype controller-type) (start-time 0) &rest args + ;; &key (clear-velocities t) &allow-other-keys) + "Send joind angle to robot with self-collision motion planning, this method retuns immediately, so use :wait-interpolation to block until the motion stops. +- av : joint angle vector [rad] +- tm : time to goal in [msec] +" + (let ((ctype controller-type) (start-time 0) (start-offset-time nil) (clear-velocities t)) + ;; as of 0.3.x, :angle-vector (robot-interface) :acceps tm ctype start-time as optional arguments, but in here we prefer old API + (if (= (length args) 1) ;; args must be ctype + (setq ctype (car args) + args (cdr args))) + (if (and (>= (length args) 2) (null (member (car args) '(:start-time :clear-velocities))));; args must be ctype start-time + (setq ctype (car args) + start-time (cadr args) + args (cddr args))) + (if (member :start-time args) (setq start-time (cadr (member :start-time args)))) + (if (member :start-offset-time args) (setq start-offset-time (cadr (member :start-offset-time args)))) + (if (member :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args)))) + ;; for simulation mode + (when (and (not (boundp '*co*)) (send self :simulation-modep)) + (return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args))) + ;; + (when (not (numberp tm)) + (ros::warn ":angle-vector tm is not a number, use :angle-vector av tm args")) + (send* self :angle-vector-motion-plan av :ctype ctype :move-arm :rarm :total-time tm + :start-offset-time (if start-offset-time start-offset-time start-time) + :clear-velocities clear-velocities args))) + (:angle-vector-sequence + (avs &optional tms &rest args) ;; (ctype controller-type) (start-time 0) &rest args + ;; &key (clear-velocities t) &allow-other-keys) + "Send joind angle to robot with self-collision motion planning, this method returns immediately, so use :wait-interpolation to block until the motion stops. +- avs : sequence of joint angle vector [rad] +- tms : list of time to goal from previous angle-vector point in [msec] +" + (let ((ctype controller-type) (start-time 0) (start-offset-time nil) (clear-velocities t)) + ;; as of 0.3.x, :angle-vector (robot-interface) :acceps tm ctype start-time as optional arguments, but in here we prefer old API + (if (= (length args) 1) ;; args must be ctype + (setq ctype (car args) + args (cdr args))) + (if (and (>= (length args) 2) (null (member (car args) '(:start-time :clear-velocities))));; args must be ctype start-time + (setq ctype (car args) + start-time (cadr args) + args (cddr args))) + (if (member :start-offset-time args) (setq start-offset-time (cadr (member :start-offset-time args)))) + (if (member :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args)))) + ;; for simulation mode + (when (send self :simulation-modep) + (return-from :angle-vector-sequence + (send* self :angle-vector-sequence-raw avs tms ctype start-time args))) + (unless (and (listp tms) (every #'numberp tms)) + (ros::warn ":angle-vector-sequence tms is not a list of number, use :angle-vector-sequence av tms args")) + (if tms + (setq tms (apply #'+ tms)) + (setq tms 3000)) + (send* self :angle-vector-motion-plan avs :ctype ctype :move-arm :rarm :total-time tms + :start-offset-time (if start-offset-time start-offset-time start-time) + :clear-velocities clear-velocities 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 '*co*) + (ros::ros-warn ";; collision-object-publisher wait for \"my_gen3/apply_planning_scene\" service for ~A sec~%" 5) + (if (ros::wait-for-service "my_gen3/apply_planning_scene" 5) + (setq *co* (instance collision-object-publisher + :init + :service-name "my_gen3/apply_planning_scene" + :scene-service "my_gen3/get_planning_scene")) + (ros::ros-warn ";; could not find \"my_gen3/apply_planning_scene\" skip creating *co*~%"))) (unless (boundp '*ri*) (setq *ri* (instance gen3-interface :init))) (ros::spin-once) @@ -67,3 +166,25 @@ (when create-viewer (objects (list *gen3*))) ) + +(defclass gen3-moveit-environment + :super moveit-environment) +(defmethod gen3-moveit-environment + (:init (&key ((:robot rb) *gen3*) &rest args) + (send-super* :init + :robot rb + :frame-id "base_link" + :scene-service "my_gen3/get_planning_scene" + :planning-service "my_gen3/plan_kinematic_path" + :execute-service "my_gen3/execute_kinematic_path" + :query-planner-interface-service "my_gen3/query_planner_interface" + :planning-scene-world "my_gen3/planning_scene_world" + :state-validity-service "my_gen3/check_state_validity" + args)) + (:default-configuration () + (list (list :rarm + (cons :group-name "arm") + (cons :target-link + (send self :search-link-from-name "end_effector_link")) + (cons :joint-list (send robot :arm :joint-list))))) + ) diff --git a/jsk_kinova_robot/kinovaeus/gen3-lite-interface.l b/jsk_kinova_robot/kinovaeus/gen3-lite-interface.l index 1a66196c15..0850a2ab60 100644 --- a/jsk_kinova_robot/kinovaeus/gen3-lite-interface.l +++ b/jsk_kinova_robot/kinovaeus/gen3-lite-interface.l @@ -1,27 +1,27 @@ (require "package://kinovaeus/gen3-lite-utils.l") (require "package://pr2eus/robot-interface.l") - -;; TODO -;; Enable to use moveit +(require "package://pr2eus_moveit/euslisp/robot-moveit.l") (defclass gen3-lite-interface :super robot-interface - :slots (gripper-action) + :slots (gripper-action moveit-robot) ) (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) + (send self :add-controller :arm-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)) + (setq moveit-robot (instance gen3_lite_gen3_lite_2f-robot :init)) + (send self :set-moveit-environment (instance gen3-lite-moveit-environment :init :robot moveit-robot)) ) - (:default-controller () (send self :rarm-controller)) - (:rarm-controller () + (:default-controller () (send self :arm-controller)) + (:arm-controller () (list (list (cons :controller-action "my_gen3_lite/gen3_lite_joint_trajectory_controller/follow_joint_trajectory") @@ -56,11 +56,110 @@ (:stop-grasp (&rest args &key &allow-other-keys) (send* self :go-grasp :pos 0.95 args)) + ;; :angle-vector-xxx and :check-continuous-joint-move-over-180 + ;; are mainly copied from package://fetcheus/fetch-interface.l + (:check-continuous-joint-move-over-180 ;; can be removed if http//github.com/jsk-ros-pkg/jsk_pr2eus/pull/322 merged + (diff-av) + (let ((i 0) add-new-trajectory-point) + (dolist (j (send robot :joint-list)) + ;; for continuous rotational joint + (when (and (> (- (send j :max-angle) (send j :min-angle)) 360) + (> (abs (elt diff-av i)) 180)) + (ros::ros-warn "continuous joint (~A) moves ~A degree, commanded joint differs from original trajectory to avoid unintentional 360 rotation" (send j :name) (elt diff-av i)) + (setq add-new-trajectory-point t)) + (incf i (send j :joint-dof))) + add-new-trajectory-point)) + (:angle-vector-raw (av &optional (tm 3000) &rest args) + (let* ((prev-av (send self :state :potentio-vector :wait-until-update t)) + (diff-av (v- av prev-av))) + (when (send self :check-continuous-joint-move-over-180 diff-av) + (let* (avs (minjerk (instance minjerk-interpolator :init)) + (scale-av (send self :sub-angle-vector av prev-av)) + dist div) + (setq dist (abs (geo::find-extream (coerce diff-av cons) #'abs #'>=))) + (setq div (round (/ dist 120.0))) + (send minjerk :reset + :position-list (list prev-av (v+ prev-av scale-av)) + :time-list (list tm)) + (send minjerk :start-interpolation) + (send minjerk :pass-time (/ tm div)) + (dotimes (i div) + (setq avs (append avs (list (send minjerk :pass-time (/ tm div)))))) + (send* self :angle-vector-sequence-raw avs (make-list div :initial-element (/ tm div)) args) + (return-from :angle-vector-raw (car (last avs))))) + (send-super* :angle-vector av tm args))) + (:angle-vector-sequence-raw (&rest args) (send-super* :angle-vector-sequence args)) + (:angle-vector + (av &optional (tm 3000) &rest args) ;; (ctype controller-type) (start-time 0) &rest args + ;; &key (clear-velocities t) &allow-other-keys) + "Send joind angle to robot with self-collision motion planning, this method retuns immediately, so use :wait-interpolation to block until the motion stops. +- av : joint angle vector [rad] +- tm : time to goal in [msec] +" + (let ((ctype controller-type) (start-time 0) (start-offset-time nil) (clear-velocities t)) + ;; as of 0.3.x, :angle-vector (robot-interface) :acceps tm ctype start-time as optional arguments, but in here we prefer old API + (if (= (length args) 1) ;; args must be ctype + (setq ctype (car args) + args (cdr args))) + (if (and (>= (length args) 2) (null (member (car args) '(:start-time :clear-velocities))));; args must be ctype start-time + (setq ctype (car args) + start-time (cadr args) + args (cddr args))) + (if (member :start-time args) (setq start-time (cadr (member :start-time args)))) + (if (member :start-offset-time args) (setq start-offset-time (cadr (member :start-offset-time args)))) + (if (member :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args)))) + ;; for simulation mode + (when (and (not (boundp '*co*)) (send self :simulation-modep)) + (return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args))) + ;; + (when (not (numberp tm)) + (ros::warn ":angle-vector tm is not a number, use :angle-vector av tm args")) + (send* self :angle-vector-motion-plan av :ctype ctype :move-arm :rarm :total-time tm + :start-offset-time (if start-offset-time start-offset-time start-time) + :clear-velocities clear-velocities args))) + (:angle-vector-sequence + (avs &optional tms &rest args) ;; (ctype controller-type) (start-time 0) &rest args + ;; &key (clear-velocities t) &allow-other-keys) + "Send joind angle to robot with self-collision motion planning, this method returns immediately, so use :wait-interpolation to block until the motion stops. +- avs : sequence of joint angle vector [rad] +- tms : list of time to goal from previous angle-vector point in [msec] +" + (let ((ctype controller-type) (start-time 0) (start-offset-time nil) (clear-velocities t)) + ;; as of 0.3.x, :angle-vector (robot-interface) :acceps tm ctype start-time as optional arguments, but in here we prefer old API + (if (= (length args) 1) ;; args must be ctype + (setq ctype (car args) + args (cdr args))) + (if (and (>= (length args) 2) (null (member (car args) '(:start-time :clear-velocities))));; args must be ctype start-time + (setq ctype (car args) + start-time (cadr args) + args (cddr args))) + (if (member :start-offset-time args) (setq start-offset-time (cadr (member :start-offset-time args)))) + (if (member :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args)))) + ;; for simulation mode + (when (send self :simulation-modep) + (return-from :angle-vector-sequence + (send* self :angle-vector-sequence-raw avs tms ctype start-time args))) + (unless (and (listp tms) (every #'numberp tms)) + (ros::warn ":angle-vector-sequence tms is not a list of number, use :angle-vector-sequence av tms args")) + (if tms + (setq tms (apply #'+ tms)) + (setq tms 3000)) + (send* self :angle-vector-motion-plan avs :ctype ctype :move-arm :rarm :total-time tms + :start-offset-time (if start-offset-time start-offset-time start-time) + :clear-velocities clear-velocities 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 '*co*) + (ros::ros-warn ";; collision-object-publisher wait for \"my_gen3_lite/apply_planning_scene\" service for ~A sec~%" 5) + (if (ros::wait-for-service "my_gen3_lite/apply_planning_scene" 5) + (setq *co* (instance collision-object-publisher + :init + :service-name "my_gen3_lite/apply_planning_scene" + :scene-service "my_gen3_lite/get_planning_scene")) + (ros::ros-warn ";; could not find \"my_gen3_lite/apply_planning_scene\" skip creating *co*~%"))) (unless (boundp '*ri*) (setq *ri* (instance gen3-lite-interface :init))) (ros::spin-once) @@ -68,3 +167,25 @@ (when create-viewer (objects (list *gen3-lite*))) ) + +(defclass gen3-lite-moveit-environment + :super moveit-environment) +(defmethod gen3-lite-moveit-environment + (:init (&key ((:robot rb) *gen3-lite*) &rest args) + (send-super* :init + :robot rb + :frame-id "base_link" + :scene-service "my_gen3_lite/get_planning_scene" + :planning-service "my_gen3_lite/plan_kinematic_path" + :execute-service "my_gen3_lite/execute_kinematic_path" + :query-planner-interface-service "my_gen3_lite/query_planner_interface" + :planning-scene-world "my_gen3_lite/planning_scene_world" + :state-validity-service "my_gen3_lite/check_state_validity" + args)) + (:default-configuration () + (list (list :rarm + (cons :group-name "arm") + (cons :target-link + (send self :search-link-from-name "end_effector_link")) + (cons :joint-list (send robot :arm :joint-list))))) + ) diff --git a/jsk_kinova_robot/kinovaeus/package.xml b/jsk_kinova_robot/kinovaeus/package.xml index 761b1c3ba5..3b5586f7cb 100644 --- a/jsk_kinova_robot/kinovaeus/package.xml +++ b/jsk_kinova_robot/kinovaeus/package.xml @@ -16,10 +16,12 @@ rostest roseus pr2eus + pr2eus_moveit kortex_description roseus pr2eus + pr2eus_moveit kortex_gazebo gen3_robotiq_2f_85_move_it_config From cb7aae98f9b55630d64f62f822e55ce5d4e4eec6 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Sun, 3 Jan 2021 17:45:52 +0900 Subject: [PATCH 05/41] Test with Gazebo --- jsk_kinova_robot/kinovaeus/CMakeLists.txt | 6 ++-- .../kinovaeus/test/test-gen3-lite-moveit.l | 31 +++++++++++++++++++ ...gazebo.test => test-gen3-lite-moveit.test} | 5 ++- .../kinovaeus/test/test-gen3-lite.test | 2 +- .../kinovaeus/test/test-gen3-moveit.l | 31 +++++++++++++++++++ ...gen3-gazebo.test => test-gen3-moveit.test} | 5 ++- .../kinovaeus/test/test-gen3.test | 2 +- 7 files changed, 75 insertions(+), 7 deletions(-) create mode 100755 jsk_kinova_robot/kinovaeus/test/test-gen3-lite-moveit.l rename jsk_kinova_robot/kinovaeus/test/{test-gen3-lite-gazebo.test => test-gen3-lite-moveit.test} (52%) create mode 100755 jsk_kinova_robot/kinovaeus/test/test-gen3-moveit.l rename jsk_kinova_robot/kinovaeus/test/{test-gen3-gazebo.test => test-gen3-moveit.test} (53%) diff --git a/jsk_kinova_robot/kinovaeus/CMakeLists.txt b/jsk_kinova_robot/kinovaeus/CMakeLists.txt index 7fa7357b93..df1ccc4098 100644 --- a/jsk_kinova_robot/kinovaeus/CMakeLists.txt +++ b/jsk_kinova_robot/kinovaeus/CMakeLists.txt @@ -62,7 +62,7 @@ if(CATKIN_ENABLE_TESTING) # 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) + # Test with Gazebo and MoveIt! + add_rostest(test/test-gen3-moveit.test) + add_rostest(test/test-gen3-lite-moveit.test) endif() diff --git a/jsk_kinova_robot/kinovaeus/test/test-gen3-lite-moveit.l b/jsk_kinova_robot/kinovaeus/test/test-gen3-lite-moveit.l new file mode 100755 index 0000000000..c0a567c597 --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/test/test-gen3-lite-moveit.l @@ -0,0 +1,31 @@ +#!/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-moveit + + (send *gen3-lite* :reset-pose) + (assert (send *ri* :angle-vector (send *gen3-lite* :angle-vector))) + (send *ri* :wait-interpolation) + + (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-raw (send *gen3-lite* :angle-vector))) + (send *ri* :wait-interpolation) + + (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* :wait-interpolation) + + (send *ri* :stop-grasp :wait t) + ) + + +(run-all-tests) +(exit) diff --git a/jsk_kinova_robot/kinovaeus/test/test-gen3-lite-gazebo.test b/jsk_kinova_robot/kinovaeus/test/test-gen3-lite-moveit.test similarity index 52% rename from jsk_kinova_robot/kinovaeus/test/test-gen3-lite-gazebo.test rename to jsk_kinova_robot/kinovaeus/test/test-gen3-lite-moveit.test index 4c2e69a5c7..3e1a40ba2a 100644 --- a/jsk_kinova_robot/kinovaeus/test/test-gen3-lite-gazebo.test +++ b/jsk_kinova_robot/kinovaeus/test/test-gen3-lite-moveit.test @@ -4,7 +4,10 @@ + + - + diff --git a/jsk_kinova_robot/kinovaeus/test/test-gen3-lite.test b/jsk_kinova_robot/kinovaeus/test/test-gen3-lite.test index a46d1ef3a1..d1c2845d30 100644 --- a/jsk_kinova_robot/kinovaeus/test/test-gen3-lite.test +++ b/jsk_kinova_robot/kinovaeus/test/test-gen3-lite.test @@ -1,3 +1,3 @@ - + diff --git a/jsk_kinova_robot/kinovaeus/test/test-gen3-moveit.l b/jsk_kinova_robot/kinovaeus/test/test-gen3-moveit.l new file mode 100755 index 0000000000..55944759d1 --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/test/test-gen3-moveit.l @@ -0,0 +1,31 @@ +#!/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-moveit + + (send *gen3* :reset-pose) + (assert (send *ri* :angle-vector (send *gen3* :angle-vector))) + (send *ri* :wait-interpolation) + + (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-raw (send *gen3* :angle-vector))) + (send *ri* :wait-interpolation) + + (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* :wait-interpolation) + + (send *ri* :stop-grasp :wait t) + ) + + +(run-all-tests) +(exit) diff --git a/jsk_kinova_robot/kinovaeus/test/test-gen3-gazebo.test b/jsk_kinova_robot/kinovaeus/test/test-gen3-moveit.test similarity index 53% rename from jsk_kinova_robot/kinovaeus/test/test-gen3-gazebo.test rename to jsk_kinova_robot/kinovaeus/test/test-gen3-moveit.test index 0603c51a26..7442091f34 100644 --- a/jsk_kinova_robot/kinovaeus/test/test-gen3-gazebo.test +++ b/jsk_kinova_robot/kinovaeus/test/test-gen3-moveit.test @@ -4,7 +4,10 @@ + + - + diff --git a/jsk_kinova_robot/kinovaeus/test/test-gen3.test b/jsk_kinova_robot/kinovaeus/test/test-gen3.test index 3d5ffc38a6..a86e827651 100644 --- a/jsk_kinova_robot/kinovaeus/test/test-gen3.test +++ b/jsk_kinova_robot/kinovaeus/test/test-gen3.test @@ -1,3 +1,3 @@ - + From bebbc72abcc9e1e47de0906b8499dbe27aa7fb34 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Sun, 3 Jan 2021 18:34:32 +0900 Subject: [PATCH 06/41] Update .rosinstall --- jsk_kinova_robot/kinova.rosinstall | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/jsk_kinova_robot/kinova.rosinstall b/jsk_kinova_robot/kinova.rosinstall index 57485d3324..160891e2f5 100644 --- a/jsk_kinova_robot/kinova.rosinstall +++ b/jsk_kinova_robot/kinova.rosinstall @@ -1,10 +1,22 @@ # TODO # Use jsk-ros-pkg/jsk_robot master after approved +# - git: +# local-name: jsk_robot +# uri: https://github.com/jsk-ros-pkg/jsk_robot.git +# version: master - git: local-name: jsk_robot uri: https://github.com/708yamaguchi/jsk_robot.git version: kinova-gen3 +# TODO +# Use Kinovarobotics/ros_kortex kinetic-devel after merging below: +# https://github.com/Kinovarobotics/ros_kortex/pull/140 +# https://github.com/Kinovarobotics/ros_kortex/pull/141 +# - git: +# local-name: ros_kortex +# uri: https://github.com/Kinovarobotics/ros_kortex +# version: kinetic-devel - git: local-name: ros_kortex - uri: https://github.com/Kinovarobotics/ros_kortex - version: kinetic-devel + uri: https://github.com/708yamaguchi/ros_kortex + version: jsk-pr From bd846c83c40eff4e95aa1d9139b09c0c80ba6065 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 4 Jan 2021 16:35:08 +0900 Subject: [PATCH 07/41] add jsk_kinova_startup --- .../jsk_kinova_startup/CMakeLists.txt | 34 ++ .../jsk_kinova_startup/config/kinova.rviz | 365 ++++++++++++++++++ .../launch/kinova_bringup.launch | 17 + .../jsk_kinova_startup/launch/rviz.launch | 5 + .../jsk_kinova_startup/package.xml | 31 ++ 5 files changed, 452 insertions(+) create mode 100644 jsk_kinova_robot/jsk_kinova_startup/CMakeLists.txt create mode 100644 jsk_kinova_robot/jsk_kinova_startup/config/kinova.rviz create mode 100644 jsk_kinova_robot/jsk_kinova_startup/launch/kinova_bringup.launch create mode 100644 jsk_kinova_robot/jsk_kinova_startup/launch/rviz.launch create mode 100644 jsk_kinova_robot/jsk_kinova_startup/package.xml diff --git a/jsk_kinova_robot/jsk_kinova_startup/CMakeLists.txt b/jsk_kinova_robot/jsk_kinova_startup/CMakeLists.txt new file mode 100644 index 0000000000..9013768fea --- /dev/null +++ b/jsk_kinova_robot/jsk_kinova_startup/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 2.8.3) +project(jsk_kinova_startup) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin) + +################################### +## catkin specific configuration ## +################################### +catkin_package() + +############# +## Install ## +############# +install(DIRECTORY config launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS) + +############# +## Testing ## +############# +if(CATKIN_ENABLE_TESTING) + find_package(catkin REQUIRED COMPONENTS roslaunch roslint) + file(GLOB LAUNCH_FILES launch/*.launch) + foreach(LAUNCH_FILE ${LAUNCH_FILES}) + roslaunch_add_file_check(${LAUNCH_FILE}) + endforeach() + + set(ROSLINT_PYTHON_OPTS --max-line-length=180 --ignore=E221,E222,E241) # skip multiple spaces before/after operator + roslint_python() + roslint_add_test() +endif() diff --git a/jsk_kinova_robot/jsk_kinova_startup/config/kinova.rviz b/jsk_kinova_robot/jsk_kinova_startup/config/kinova.rviz new file mode 100644 index 0000000000..44b72db5fd --- /dev/null +++ b/jsk_kinova_robot/jsk_kinova_startup/config/kinova.rviz @@ -0,0 +1,365 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 344 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Image +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/Axes + Enabled: true + Length: 0.5 + Name: Axes + Radius: 0.05000000074505806 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bracelet_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + end_effector_link: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + half_arm_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + half_arm_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_inner_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_inner_finger_pad: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_inner_knuckle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_outer_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_outer_knuckle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_inner_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_inner_finger_pad: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_inner_knuckle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_outer_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_outer_knuckle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_arg2f_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spherical_wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spherical_wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + bracelet_link: + Value: true + camera_color_frame: + Value: true + camera_depth_frame: + Value: true + camera_link: + Value: true + end_effector_link: + Value: true + forearm_link: + Value: true + half_arm_1_link: + Value: true + half_arm_2_link: + Value: true + left_inner_finger: + Value: true + left_inner_finger_pad: + Value: true + left_inner_knuckle: + Value: true + left_outer_finger: + Value: true + left_outer_knuckle: + Value: true + right_inner_finger: + Value: true + right_inner_finger_pad: + Value: true + right_inner_knuckle: + Value: true + right_outer_finger: + Value: true + right_outer_knuckle: + Value: true + robotiq_arg2f_base_link: + Value: true + shoulder_link: + Value: true + spherical_wrist_1_link: + Value: true + spherical_wrist_2_link: + Value: true + tool_frame: + Value: true + Marker Scale: 0.4000000059604645 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + base_link: + shoulder_link: + half_arm_1_link: + half_arm_2_link: + forearm_link: + spherical_wrist_1_link: + spherical_wrist_2_link: + bracelet_link: + end_effector_link: + camera_color_frame: + {} + camera_depth_frame: + {} + camera_link: + {} + robotiq_arg2f_base_link: + left_inner_knuckle: + {} + left_outer_knuckle: + left_outer_finger: + left_inner_finger: + left_inner_finger_pad: + {} + right_inner_knuckle: + {} + right_outer_knuckle: + right_outer_finger: + right_inner_finger: + right_inner_finger_pad: + {} + tool_frame: + {} + Update Interval: 0 + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /camera/color/image_rect_color + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 3.203127384185791 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.08768113702535629 + Y: -0.21367596089839935 + Z: 0.5479295253753662 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.38539838790893555 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.2504054307937622 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001f3000002b0fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001e3000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000226000000c70000001600fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670200000532000000c2000001f50000018a000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002b7000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1200 + X: 67 + Y: 27 diff --git a/jsk_kinova_robot/jsk_kinova_startup/launch/kinova_bringup.launch b/jsk_kinova_robot/jsk_kinova_startup/launch/kinova_bringup.launch new file mode 100644 index 0000000000..d09f25ca4e --- /dev/null +++ b/jsk_kinova_robot/jsk_kinova_startup/launch/kinova_bringup.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/jsk_kinova_robot/jsk_kinova_startup/launch/rviz.launch b/jsk_kinova_robot/jsk_kinova_startup/launch/rviz.launch new file mode 100644 index 0000000000..394aad4719 --- /dev/null +++ b/jsk_kinova_robot/jsk_kinova_startup/launch/rviz.launch @@ -0,0 +1,5 @@ + + + diff --git a/jsk_kinova_robot/jsk_kinova_startup/package.xml b/jsk_kinova_robot/jsk_kinova_startup/package.xml new file mode 100644 index 0000000000..d26f46b192 --- /dev/null +++ b/jsk_kinova_robot/jsk_kinova_startup/package.xml @@ -0,0 +1,31 @@ + + jsk_kinova_startup + 1.1.0 + + jsk_kinova_startup + + Kei Okada + BSD + http://ros.org/wiki/jsk_kinova_startup + catkin + + kinova_teleop + + jsk_robot_startup + kortex_driver + kinova_vision + kinovaeus + robot_state_publisher + rviz + + + diagnostic_aggregator + + roslaunch + roslint + rostest + + + + + From 1fcecbed99fd1f18fc19ad0da6f4ae1a4b66390c Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 4 Jan 2021 16:39:56 +0900 Subject: [PATCH 08/41] add ros_kortex_vision to .rosinstall --- jsk_kinova_robot/kinova.rosinstall | 3 +++ 1 file changed, 3 insertions(+) diff --git a/jsk_kinova_robot/kinova.rosinstall b/jsk_kinova_robot/kinova.rosinstall index 160891e2f5..6243506026 100644 --- a/jsk_kinova_robot/kinova.rosinstall +++ b/jsk_kinova_robot/kinova.rosinstall @@ -20,3 +20,6 @@ local-name: ros_kortex uri: https://github.com/708yamaguchi/ros_kortex version: jsk-pr +- git: + local-name: ros_kortex_vision + uri: https://github.com/Kinovarobotics/ros_kortex_vision From b1fd2bbdd3423cdc839ad9c9ec0fed2ac48f8fd6 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 4 Jan 2021 16:41:19 +0900 Subject: [PATCH 09/41] update CMakeLists.txt use foreach to compile robots --- jsk_kinova_robot/kinovaeus/CMakeLists.txt | 40 +++++++++++------------ 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/jsk_kinova_robot/kinovaeus/CMakeLists.txt b/jsk_kinova_robot/kinovaeus/CMakeLists.txt index df1ccc4098..25766ab059 100644 --- a/jsk_kinova_robot/kinovaeus/CMakeLists.txt +++ b/jsk_kinova_robot/kinovaeus/CMakeLists.txt @@ -1,11 +1,14 @@ cmake_minimum_required(VERSION 2.8.3) project(kinovaeus) +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS roseus euscollada - kortex_description ) +find_package(kortex_description) # Just in case when description is not released. Avoid compile failing catkin_package() @@ -30,35 +33,30 @@ 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) +foreach(ROBOT_TYPE gen3_lite_gen3_lite_2f gen3_robotiq_2f_85 gen3_robotiq_2f_140) + add_custom_command(OUTPUT ${PROJECT_BINARY_DIR}/${ROBOT_TYPE}.urdf + COMMAND xacro ${_kinova_urdf}/${ROBOT_TYPE}.xacro sim:=false > ${PROJECT_BINARY_DIR}/${ROBOT_TYPE}.urdf + DEPENDS ${_kinova_urdf}/${ROBOT_TYPE}.xacro) + add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/${ROBOT_TYPE}.l + COMMAND echo "${_collada2eus} ${PROJECT_BINARY_DIR}/${ROBOT_TYPE}.urdf ${ROBOT_TYPE}.l" + COMMAND ${_collada2eus} ${PROJECT_BINARY_DIR}/${ROBOT_TYPE}.urdf ${PROJECT_SOURCE_DIR}/${ROBOT_TYPE}.yaml ${PROJECT_SOURCE_DIR}/${ROBOT_TYPE}.l + DEPENDS ${PROJECT_BINARY_DIR}/${ROBOT_TYPE}.urdf ${PROJECT_SOURCE_DIR}/${ROBOT_TYPE}.yaml ${_collada2eus}) + add_custom_target(compile_${ROBOT_TYPE} ALL DEPENDS ${PROJECT_SOURCE_DIR}/${ROBOT_TYPE}.l) +endforeach() + 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}) +install(FILES kinova.l kinova-interface.l kinova-util.l DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) if(CATKIN_ENABLE_TESTING) find_package(catkin REQUIRED COMPONENTS rostest) + # Test only roseus + add_rostest(test/test-kinova.test) # Test with Kinematic Simulator add_rostest(test/test-gen3.test) add_rostest(test/test-gen3-lite.test) @@ -66,3 +64,5 @@ if(CATKIN_ENABLE_TESTING) add_rostest(test/test-gen3-moveit.test) add_rostest(test/test-gen3-lite-moveit.test) endif() + + From 187ad3653be911dbcfe3d50c316b3b69ed92736f Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 4 Jan 2021 16:42:42 +0900 Subject: [PATCH 10/41] update README --- jsk_kinova_robot/README.md | 80 +++++++++++++++++++++++++------------- 1 file changed, 52 insertions(+), 28 deletions(-) diff --git a/jsk_kinova_robot/README.md b/jsk_kinova_robot/README.md index 6636b68aa6..918f5b2d90 100644 --- a/jsk_kinova_robot/README.md +++ b/jsk_kinova_robot/README.md @@ -10,6 +10,20 @@ ROS package for KINOVA Gen3/Gen3 Lite robot. ## How to setup development environment +### Conan Setup + +You need to install `conan` (Decentralized, open-source (MIT), C/C++ package manager) to build kinova packages + +```bash +sudo apt install python3 python3-pip +python3 -m pip install --user conan +conan config set general.revisions_enabled=1 +conan profile new default --detect > /dev/null +conan profile update settings.compiler.libcxx=libstdc++11 default +``` + +### ROS Environment Setup + Use `wstool`, `rosdep` and `catkin` to checkout and compile the source tree. ```bash @@ -22,74 +36,82 @@ wstool update cd ../ source /opt/ros/melodic/setup.bash rosdep install -y -r --from-paths src --ignore-src -# TODO: add catkin build jsk_kinova_startup -catkin build kinovaeus +catkin build jsk_kinova_startup 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 +roslaunch jsk_kinova_startup kinova_bringup.launch +``` + +Start rviz from different terminal + +```bash +roslaunch jsk_kinova_startup rvi.launch ``` ## 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) +(load "package://kinovaeus/kinova-interface.l") +(kinova-init) ;; Gen3 Lite -(load "package://kinovaeus/gen3-lite-interface.l") -(gen3-lite-init) +(load "package://kinovaeus/kinova-lite-interface.l") +(kinova-init :type :gen3_lite_gen3_lite_2f) + +## Use EusLisp model + +To control the robot form EusLisp. Please start `roseus` and type as follows. + +``` +(load "package://kinovaeus/kinova-interface.l") +(kinova-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)) +(send *kinova* :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) +(send *kinova* :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)) +(send *kinova* :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) +(send *kinova* :arm :move-end-rot -90 :z) ``` To control real robot. you can use *ri* object. ``` -(send *ri* :angle-vector (send *gen3* :angle-vector) 2000) +(send *ri* :angle-vector (send *kinova* :angle-vector) 2000) ``` 2000 indicates we ask robot to move for 2000 [msec] To obtain current robot pose, use :state :potentio-vector method. +``` +(send *kinova* :arm :move-end-pos #f(0 0 -50)) +``` + +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. +To open and close the gripper. You can use `:start-grasp` and `:stop-grasp`. + ``` (send *ri* :stop-grasp) (send *ri* :start-grasp) @@ -102,7 +124,8 @@ Kinova Gen3 robot with robotiq 2f 85 gripper. roslaunch kortex_gazebo spawn_kortex_robot.launch arm:=gen3 gripper:=robotiq_2f_85 # Terminal 2 roscd kinovaeus -roseus gen3-interface.l +roseus kinova-interface.l +(kinova-init :type :gen3_robotiq_2f_85) ``` Kinova Gen3 Lite robot with lite 2f gripper. @@ -111,8 +134,9 @@ Kinova Gen3 Lite robot with lite 2f gripper. roslaunch kortex_gazebo spawn_kortex_robot.launch arm:=gen3_lite gripper:=gen3_lite_2f # Terminal 2 roscd kinovaeus -roseus gen3-lite-interface.l -``` +roseus kinova-interface.l +(kinova-init :type :gen3_lite_gen3_lite_2f) +```` --- If you have any question, please feel free to file open at https://github.com/jsk-ros-pkg/jsk_robot/issues From 8bb4170e83ca974b82ebb87f657eb33af5bc7b57 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 4 Jan 2021 16:43:05 +0900 Subject: [PATCH 11/41] update .gitignore --- jsk_kinova_robot/kinovaeus/.gitignore | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/jsk_kinova_robot/kinovaeus/.gitignore b/jsk_kinova_robot/kinovaeus/.gitignore index 8d29999b25..4b6ad1b0d7 100644 --- a/jsk_kinova_robot/kinovaeus/.gitignore +++ b/jsk_kinova_robot/kinovaeus/.gitignore @@ -1,4 +1,3 @@ -gen3.l -gen3-lite.l -gen3.urdf -gen3-lite.urdf +gen3_lite_gen3_lite_2f.l +gen3_robotiq_2f_140.l +gen3_robotiq_2f_85.l From 826c64ac7decea665b09ba34b33a7f8d5f3558e4 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 4 Jan 2021 16:44:46 +0900 Subject: [PATCH 12/41] add robotiq_2f_140 to test_depend --- jsk_kinova_robot/kinovaeus/package.xml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/jsk_kinova_robot/kinovaeus/package.xml b/jsk_kinova_robot/kinovaeus/package.xml index 3b5586f7cb..62cff09b9b 100644 --- a/jsk_kinova_robot/kinovaeus/package.xml +++ b/jsk_kinova_robot/kinovaeus/package.xml @@ -1,8 +1,8 @@ kinovaeus - 1.0.0 - The kinova package + 1.1.0 + The kinovaeus package Naoya Yamaguchi @@ -25,6 +25,7 @@ kortex_gazebo gen3_robotiq_2f_85_move_it_config + gen3_robotiq_2f_140_move_it_config gen3_lite_gen3_lite_2f_move_it_config From 2a88c960afaff1e50633a5fcbb74902756906dcf Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 4 Jan 2021 16:51:00 +0900 Subject: [PATCH 13/41] use collar for 1st joint, use tool\frame for end-coords --- .../kinovaeus/gen3_lite_gen3_lite_2f.yaml | 20 ++++++++++++++++++ .../kinovaeus/gen3_robotiq_2f_140.yaml | 21 +++++++++++++++++++ .../kinovaeus/gen3_robotiq_2f_85.yaml | 21 +++++++++++++++++++ 3 files changed, 62 insertions(+) create mode 100644 jsk_kinova_robot/kinovaeus/gen3_lite_gen3_lite_2f.yaml create mode 100644 jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_140.yaml create mode 100644 jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_85.yaml diff --git a/jsk_kinova_robot/kinovaeus/gen3_lite_gen3_lite_2f.yaml b/jsk_kinova_robot/kinovaeus/gen3_lite_gen3_lite_2f.yaml new file mode 100644 index 0000000000..b22a9e1494 --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/gen3_lite_gen3_lite_2f.yaml @@ -0,0 +1,20 @@ +## +## - collada_joint_name : euslisp_joint_name (start with :) +## + +rarm: + - joint_1 : rarm-collar-y + - joint_2 : rarm-shoulder-p + - joint_3 : rarm-shoulder-y + - joint_4 : rarm-elbow-p + - joint_5 : rarm-elbow-y + - 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 : tool_frame + translate : [0, 0, 0] + rotate : [-0.57735, -0.57735, -0.57735, 120] diff --git a/jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_140.yaml b/jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_140.yaml new file mode 100644 index 0000000000..c340947654 --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_140.yaml @@ -0,0 +1,21 @@ +## +## - collada_joint_name : euslisp_joint_name (start with :) +## + +rarm: + - joint_1 : rarm-collar-y + - joint_2 : rarm-shoulder-p + - joint_3 : rarm-shoulder-y + - joint_4 : rarm-elbow-p + - joint_5 : rarm-elbow-y + - 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, -20.0, 180.0, -140.0, 0.0, 30.0, 90.0] + +rarm-end-coords: + parent : tool_frame + translate : [0, 0, 0] + rotate : [-0.57735, -0.57735, -0.57735, 120] diff --git a/jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_85.yaml b/jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_85.yaml new file mode 100644 index 0000000000..c340947654 --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_85.yaml @@ -0,0 +1,21 @@ +## +## - collada_joint_name : euslisp_joint_name (start with :) +## + +rarm: + - joint_1 : rarm-collar-y + - joint_2 : rarm-shoulder-p + - joint_3 : rarm-shoulder-y + - joint_4 : rarm-elbow-p + - joint_5 : rarm-elbow-y + - 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, -20.0, 180.0, -140.0, 0.0, 30.0, 90.0] + +rarm-end-coords: + parent : tool_frame + translate : [0, 0, 0] + rotate : [-0.57735, -0.57735, -0.57735, 120] From 97e9ccd9852358946c949383cd2d553a57cc3544 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 4 Jan 2021 16:51:49 +0900 Subject: [PATCH 14/41] rename gen3-interface to kinova-interface/kinova-utils, and add kinova.l --- jsk_kinova_robot/kinovaeus/kinova-interface.l | 263 ++++++++++++++++++ jsk_kinova_robot/kinovaeus/kinova-utils.l | 1 + jsk_kinova_robot/kinovaeus/kinova.l | 64 +++++ 3 files changed, 328 insertions(+) create mode 100644 jsk_kinova_robot/kinovaeus/kinova-interface.l create mode 100644 jsk_kinova_robot/kinovaeus/kinova-utils.l create mode 100644 jsk_kinova_robot/kinovaeus/kinova.l diff --git a/jsk_kinova_robot/kinovaeus/kinova-interface.l b/jsk_kinova_robot/kinovaeus/kinova-interface.l new file mode 100644 index 0000000000..6e1c98f4ec --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/kinova-interface.l @@ -0,0 +1,263 @@ +(require "package://kinovaeus/kinova-utils.l") +(require "package://pr2eus/robot-interface.l") + +(if (not (find-package "kinova")) + (make-package "kinova")) + +(defclass kinova-interface + :super robot-interface + :slots (controller-handle robot-handle move-robot) + ) + +(defmethod kinova-interface + (:init + (&key type &rest args) + (let () + (prog1 + (send-super* :init :robot (apply #'kinova (if type (list :type type) nil)) :joint-states-topic "/arm_gen3/joint_states" :groupname "kinova_interface" args) + (send self :add-controller :arm-controller) + (setq gripper-action + (instance ros::simple-action-client :init + (cond ((derivedp robot gen3_robotiq_2f_140-robot) + "/arm_gen3/robotiq_2f_140_gripper_controller/gripper_cmd") + ((derivedp robot gen3_robotiq_2f_85-robot) + "/arm_gen3/robotiq_2f_85_gripper_controller/gripper_cmd") + ((derivedp robot gen3_robotiq_2f_140-robot) + "/arm_gen3/gen3_lite_2f_gripper_gripper_controller/gripper_cmd") + (t + (warning-message 1 "unknown kinova robot type ~A~%" robot) + )) + control_msgs::GripperCommandAction + :groupname groupname)) + (setq moveit-robot (instance (eval (send (class robot) :name)) :init)) + (send self :set-moveit-environment (instance gen3-moveit-environment :init :robot moveit-robot)) + self))) + ;; default robot-interface methods + (:default-controller () (send self :arm-controller)) + (:arm-controller () + (list + (list + (cons :controller-action "arm_gen3/gen3_joint_trajectory_controller/follow_joint_trajectory") + (cons :controller-state "arm_gen3/gen3_joint_trajectory_controller/state") ;; @TODO: This is dummy topoic name. The kinova_driver did not publish JointTrajectoryControllerState + (cons :action-type control_msgs::FollowJointTrajectoryAction) + (cons :joint-names (send-all (send robot :joint-list) :name))))) + (:go-grasp + (&rest args) + (cond ((derivedp robot gen3_robotiq_2f_140-robot) + (send* self :go-grasp-robotiq args)) + ((derivedp robot gen3_robotiq_2f_85-robot) + (send* self :go-grasp-robotiq args)) + ((derivedp robot gen3_robotiq_2f_140-robot) + (send* self :go-grasp-gen3-lite args)) + (t + (warning-message 1 "unknown kinova robot type ~A~%" robot)))) + (:start-grasp + (&rest args) + (cond ((derivedp robot gen3_robotiq_2f_140-robot) + (send* self :start-grasp-robotiq args)) + ((derivedp robot gen3_robotiq_2f_85-robot) + (send* self :start-grasp-robotiq args)) + ((derivedp robot gen3_robotiq_2f_140-robot) + (send* self :start-grasp-gen3-lite args)) + (t + (warning-message 1 "unknown kinova robot type ~A~%" robot)))) + (:stop-grasp + (&rest args) + (cond ((derivedp robot gen3_robotiq_2f_140-robot) + (send* self :stop-grasp-robotiq args)) + ((derivedp robot gen3_robotiq_2f_85-robot) + (send* self :stop-grasp-robotiq args)) + ((derivedp robot gen3_robotiq_2f_140-robot) + (send* self :stop-grasp-gen3-lite args)) + (t + (warning-message 1 "unknown kinova robot type ~A~%" robot)))) + ;; grasp for robotiq + (:go-grasp-robotiq (&key (pos 0.0) (wait t)) + (when (send self :simulation-modep) + (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-robotiq 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-robotiq + (&rest args &key &allow-other-keys) + (send* self :go-grasp-robotiq :pos 0.8 args)) + (:stop-grasp-robotiq + (&rest args &key &allow-other-keys) + (send* self :go-grasp-robotiq :pos 0.0 args)) + ;; grasp for gen3 lite + (:go-grasp-lite (&key (pos 0.0) (wait t)) + (when (send self :simulation-modep) + (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-lite t)) + (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))))) + (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-lite + (&rest args &key &allow-other-keys) + (send* self :go-grasp-lite :pos -0.05 args)) + (:stop-grasp-lite + (&rest args &key &allow-other-keys) + (send* self :go-grasp-lite :pos 0.95 args)) + ;; :angle-vector-xxx and :check-continuous-joint-move-over-180 + ;; are mainly copied from package://fetcheus/fetch-interface.l + (:check-continuous-joint-move-over-180 ;; can be removed if http//github.com/jsk-ros-pkg/jsk_pr2eus/pull/322 merged + (diff-av) + (let ((i 0) add-new-trajectory-point) + (dolist (j (send robot :joint-list)) + ;; for continuous rotational joint + (when (and (> (- (send j :max-angle) (send j :min-angle)) 360) + (> (abs (elt diff-av i)) 180)) + (ros::ros-warn "continuous joint (~A) moves ~A degree, commanded joint differs from original trajectory to avoid unintentional 360 rotation" (send j :name) (elt diff-av i)) + (setq add-new-trajectory-point t)) + (incf i (send j :joint-dof))) + add-new-trajectory-point)) + (:angle-vector-raw (av &optional (tm 3000) &rest args) + (let* ((prev-av (send self :state :potentio-vector :wait-until-update t)) + (diff-av (v- av prev-av))) + (when (send self :check-continuous-joint-move-over-180 diff-av) + (let* (avs (minjerk (instance minjerk-interpolator :init)) + (scale-av (send self :sub-angle-vector av prev-av)) + dist div) + (setq dist (abs (geo::find-extream (coerce diff-av cons) #'abs #'>=))) + (setq div (round (/ dist 120.0))) + (send minjerk :reset + :position-list (list prev-av (v+ prev-av scale-av)) + :time-list (list tm)) + (send minjerk :start-interpolation) + (send minjerk :pass-time (/ tm div)) + (dotimes (i div) + (setq avs (append avs (list (send minjerk :pass-time (/ tm div)))))) + (send* self :angle-vector-sequence-raw avs (make-list div :initial-element (/ tm div)) args) + (return-from :angle-vector-raw (car (last avs))))) + (send-super* :angle-vector av tm args))) + (:angle-vector-sequence-raw (&rest args) (send-super* :angle-vector-sequence args)) + (:angle-vector + (av &optional (tm 3000) &rest args) ;; (ctype controller-type) (start-time 0) &rest args + ;; &key (clear-velocities t) &allow-other-keys) + "Send joind angle to robot with self-collision motion planning, this method retuns immediately, so use :wait-interpolation to block until the motion stops. +- av : joint angle vector [rad] +- tm : time to goal in [msec] +" + (let ((ctype controller-type) (start-time 0) (start-offset-time nil) (clear-velocities t)) + ;; as of 0.3.x, :angle-vector (robot-interface) :acceps tm ctype start-time as optional arguments, but in here we prefer old API + (if (= (length args) 1) ;; args must be ctype + (setq ctype (car args) + args (cdr args))) + (if (and (>= (length args) 2) (null (member (car args) '(:start-time :clear-velocities))));; args must be ctype start-time + (setq ctype (car args) + start-time (cadr args) + args (cddr args))) + (if (member :start-time args) (setq start-time (cadr (member :start-time args)))) + (if (member :start-offset-time args) (setq start-offset-time (cadr (member :start-offset-time args)))) + (if (member :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args)))) + ;; for simulation mode + (when (and (not (boundp '*co*)) (send self :simulation-modep)) + (return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args))) + ;; + (when (not (numberp tm)) + (ros::warn ":angle-vector tm is not a number, use :angle-vector av tm args")) + (send* self :angle-vector-motion-plan av :ctype ctype :move-arm :rarm :total-time tm + :start-offset-time (if start-offset-time start-offset-time start-time) + :clear-velocities clear-velocities args))) + (:angle-vector-sequence + (avs &optional tms &rest args) ;; (ctype controller-type) (start-time 0) &rest args + ;; &key (clear-velocities t) &allow-other-keys) + "Send joind angle to robot with self-collision motion planning, this method returns immediately, so use :wait-interpolation to block until the motion stops. +- avs : sequence of joint angle vector [rad] +- tms : list of time to goal from previous angle-vector point in [msec] +" + (let ((ctype controller-type) (start-time 0) (start-offset-time nil) (clear-velocities t)) + ;; as of 0.3.x, :angle-vector (robot-interface) :acceps tm ctype start-time as optional arguments, but in here we prefer old API + (if (= (length args) 1) ;; args must be ctype + (setq ctype (car args) + args (cdr args))) + (if (and (>= (length args) 2) (null (member (car args) '(:start-time :clear-velocities))));; args must be ctype start-time + (setq ctype (car args) + start-time (cadr args) + args (cddr args))) + (if (member :start-offset-time args) (setq start-offset-time (cadr (member :start-offset-time args)))) + (if (member :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args)))) + ;; for simulation mode + (when (send self :simulation-modep) + (return-from :angle-vector-sequence + (send* self :angle-vector-sequence-raw avs tms ctype start-time args))) + (unless (and (listp tms) (every #'numberp tms)) + (ros::warn ":angle-vector-sequence tms is not a list of number, use :angle-vector-sequence av tms args")) + (if tms + (setq tms (apply #'+ tms)) + (setq tms 3000)) + (send* self :angle-vector-motion-plan avs :ctype ctype :move-arm :rarm :total-time tms + :start-offset-time (if start-offset-time start-offset-time start-time) + :clear-velocities clear-velocities args))) + ) + +(defclass kinova-moveit-environment + :super moveit-environment) +(defmethod kinova-moveit-environment + (:init (&key ((:robot rb) *gen3*) &rest args) + (send-super* :init + :robot rb + :frame-id "base_link" + :scene-service "arm_gen3/get_planning_scene" + :planning-service "arm_gen3/plan_kinematic_path" + :execute-service "arm_gen3/execute_kinematic_path" + :query-planner-interface-service "arm_gen3/query_planner_interface" + :planning-scene-world "arm_gen3/planning_scene_world" + :state-validity-service "arm_gen3/check_state_validity" + args)) + (:default-configuration () + (list (list :rarm + (cons :group-name "arm") + (cons :target-link + (send self :search-link-from-name "end_effector_link")) + (cons :joint-list (send robot :arm :joint-list))))) + ) + +(defun kinova-init (&key (type nil) &rest args &alow-other-keys) + (unless (boundp '*kinova*) (apply #'kinova (if type (list :type type) nil)) (send *kinova* :reset-pose)) + (unless (ros::ok) (ros::roseus "kinova_eus_interface")) + (unless (boundp '*co*) + (ros::ros-warn ";; collision-object-publisher wait for \"arm_gen3/apply_planning_scene\" service for ~A sec~%" 5) + (if (ros::wait-for-service "arm_gen3/apply_planning_scene" 5) + (setq *co* (instance collision-object-publisher + :init + :service-name "arm_gen3/apply_planning_scene" + :scene-service "arm_gen3/get_planning_scene")) + (ros::ros-warn ";; could not find \"arm_gen3/apply_planning_scene\" skip creating *co*~%"))) + (unless (boundp '*ri*) (setq *ri* (instance kinova-interface :init :type type))) + + (ros::spin-once) + (send *ri* :spin-once) + + ;;(when create-viewer (objects (list *kinova*))) + ) diff --git a/jsk_kinova_robot/kinovaeus/kinova-utils.l b/jsk_kinova_robot/kinovaeus/kinova-utils.l new file mode 100644 index 0000000000..23be601618 --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/kinova-utils.l @@ -0,0 +1 @@ +(require :kinova "package://kinovaeus/kinova.l") diff --git a/jsk_kinova_robot/kinovaeus/kinova.l b/jsk_kinova_robot/kinovaeus/kinova.l new file mode 100644 index 0000000000..f7274182bf --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/kinova.l @@ -0,0 +1,64 @@ +(require :gen3_robotiq_2f_85 "package://kinovaeus/gen3_robotiq_2f_85.l") +(require :gen3_robotiq_2f_140 "package://kinovaeus/gen3_robotiq_2f_140.l") +(require :gen3_lite_gen3_lite_2f "package://kinovaeus/gen3_lite_gen3_lite_2f.l") + +(defun kinova (&key (type :gen3_robotiq_2f_140) &rest args &allow-other-keys) + (setq *kinova* (case type + (:gen3_robotiq_2f_85 + (instance* gen3_robotiq_2f_85-robot :init args)) + (:gen3_robotiq_2f_140 + (instance* gen3_robotiq_2f_140-robot :init args)) + (:gen3_lite_gen3_lite_2f + (instance* gen3_lite_gen3_lite_2f-robot :init args)) + (t + (warning-message 1 "unknown kinova robot type ~A~%" type) + ))) + *kinova*) + +(defmethod gen3_robotiq_2f_85-robot + (:arm (&rest args) (send* self :rarm args)) ;; enable to call send *kinova* :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)) + ) +(defmethod gen3_robotiq_2f_140-robot + (:arm (&rest args) (send* self :rarm args)) ;; enable to call send *kinova* :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)) + ) +(defmethod gen3_lite_gen3_lite_2f-robot + (:arm (&rest args) (send* self :rarm args)) ;; enable to call send *kinova* :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)) + ) + From 840ba1c9b512c86402cd04a5a7c0cdf51313bf9b Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 4 Jan 2021 16:52:12 +0900 Subject: [PATCH 15/41] add test codes for eusmodel --- jsk_kinova_robot/kinovaeus/test/test-kinova.l | 30 +++++++++++++++++++ .../kinovaeus/test/test-kinova.test | 3 ++ 2 files changed, 33 insertions(+) create mode 100755 jsk_kinova_robot/kinovaeus/test/test-kinova.l create mode 100644 jsk_kinova_robot/kinovaeus/test/test-kinova.test diff --git a/jsk_kinova_robot/kinovaeus/test/test-kinova.l b/jsk_kinova_robot/kinovaeus/test/test-kinova.l new file mode 100755 index 0000000000..436b9de644 --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/test/test-kinova.l @@ -0,0 +1,30 @@ +#!/usr/bin/env roseus +(require :unittest "lib/llib/unittest.l") +(require "package://kinovaeus/kinova.l") +;; (require "package://kinovaeus/kinova-interface.l") + +(init-unit-test) + +(deftest test-pose + (let (robot) + (setq robot (kinova)) + (objects (list robot)) + ;; rotation-angle NIL is returned when rotation-matrix is a unit-matrix + (warning-message 2 "~A end-corods ~A~%" (send robot :name) (rotation-angle (send robot :arm :end-coords :worldrot))) + (assert (eps= (norm (send (make-coords) :difference-rotation (send robot :arm :end-coords))) 0)) + + (setq robot (kinova :type :gen3_robotiq_2f_140)) + (objects (list robot)) + (warning-message 2 "~A end-corods ~A~%" (send robot :name) (rotation-angle (send robot :arm :end-coords :worldrot))) + (assert (eps= (norm (send (make-coords) :difference-rotation (send robot :arm :end-coords))) 0)) + + (setq robot (kinova :type :gen3_lite_gen3_lite_2f)) + (objects (list robot)) + (warning-message 2 "~A end-corods ~A~%" (send robot :name) (rotation-angle (send robot :arm :end-coords :worldrot))) + (assert (eps= (norm (send (make-coords) :difference-rotation (send robot :arm :end-coords))) 0)) + )) + + +(run-all-tests) +(exit) + diff --git a/jsk_kinova_robot/kinovaeus/test/test-kinova.test b/jsk_kinova_robot/kinovaeus/test/test-kinova.test new file mode 100644 index 0000000000..c516ac8d84 --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/test/test-kinova.test @@ -0,0 +1,3 @@ + + + From a74b702b8b6dc9bec8c32de91e0c56723daa24e6 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Mon, 4 Jan 2021 19:14:09 +0900 Subject: [PATCH 16/41] fix bugs --- jsk_kinova_robot/README.md | 7 +- jsk_kinova_robot/kinovaeus/gen3-interface.l | 190 ----------------- .../kinovaeus/gen3-lite-interface.l | 191 ------------------ jsk_kinova_robot/kinovaeus/gen3-lite-utils.l | 20 -- jsk_kinova_robot/kinovaeus/gen3-lite.yaml | 20 -- jsk_kinova_robot/kinovaeus/gen3-utils.l | 19 -- jsk_kinova_robot/kinovaeus/gen3.yaml | 21 -- .../kinovaeus/gen3_lite_gen3_lite_2f.yaml | 2 + .../kinovaeus/gen3_robotiq_2f_140.yaml | 4 +- .../kinovaeus/gen3_robotiq_2f_85.yaml | 4 +- jsk_kinova_robot/kinovaeus/kinova-interface.l | 23 ++- 11 files changed, 28 insertions(+), 473 deletions(-) delete mode 100644 jsk_kinova_robot/kinovaeus/gen3-interface.l delete mode 100644 jsk_kinova_robot/kinovaeus/gen3-lite-interface.l delete mode 100644 jsk_kinova_robot/kinovaeus/gen3-lite-utils.l delete mode 100644 jsk_kinova_robot/kinovaeus/gen3-lite.yaml delete mode 100644 jsk_kinova_robot/kinovaeus/gen3-utils.l delete mode 100644 jsk_kinova_robot/kinovaeus/gen3.yaml diff --git a/jsk_kinova_robot/README.md b/jsk_kinova_robot/README.md index 918f5b2d90..7667eb7f6b 100644 --- a/jsk_kinova_robot/README.md +++ b/jsk_kinova_robot/README.md @@ -63,6 +63,7 @@ To control the robot form EusLisp. Please start `roseus` and type as follows. ;; Gen3 Lite (load "package://kinovaeus/kinova-lite-interface.l") (kinova-init :type :gen3_lite_gen3_lite_2f) +``` ## Use EusLisp model @@ -121,7 +122,7 @@ To open and close the gripper. You can use `:start-grasp` and `:stop-grasp`. Kinova Gen3 robot with robotiq 2f 85 gripper. ```bash # Terminal 1 -roslaunch kortex_gazebo spawn_kortex_robot.launch arm:=gen3 gripper:=robotiq_2f_85 +roslaunch kortex_gazebo spawn_kortex_robot.launch arm:=gen3 gripper:=robotiq_2f_85 robot_name:=arm_gen3 # Terminal 2 roscd kinovaeus roseus kinova-interface.l @@ -131,12 +132,12 @@ roseus kinova-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 +roslaunch kortex_gazebo spawn_kortex_robot.launch arm:=gen3_lite gripper:=gen3_lite_2f robot_name:=arm_gen3 # Terminal 2 roscd kinovaeus roseus kinova-interface.l (kinova-init :type :gen3_lite_gen3_lite_2f) -```` +``` --- 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/kinovaeus/gen3-interface.l b/jsk_kinova_robot/kinovaeus/gen3-interface.l deleted file mode 100644 index 74a6a9c5d0..0000000000 --- a/jsk_kinova_robot/kinovaeus/gen3-interface.l +++ /dev/null @@ -1,190 +0,0 @@ -(require "package://kinovaeus/gen3-utils.l") -(require "package://pr2eus/robot-interface.l") -(require "package://pr2eus_moveit/euslisp/robot-moveit.l") - -(defclass gen3-interface - :super robot-interface - :slots (gripper-action moveit-robot) - ) - -(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 :arm-controller) - (setq gripper-action - (instance ros::simple-action-client :init - "/my_gen3/robotiq_2f_85_gripper_controller/gripper_cmd" - control_msgs::GripperCommandAction - :groupname groupname)) - (setq moveit-robot (instance gen3_robotiq_2f_85-robot :init)) - (send self :set-moveit-environment (instance gen3-moveit-environment :init :robot moveit-robot)) - ) - (:default-controller () (send self :arm-controller)) - (:arm-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) - (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)) - (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)) - ;; :angle-vector-xxx and :check-continuous-joint-move-over-180 - ;; are mainly copied from package://fetcheus/fetch-interface.l - (:check-continuous-joint-move-over-180 ;; can be removed if http//github.com/jsk-ros-pkg/jsk_pr2eus/pull/322 merged - (diff-av) - (let ((i 0) add-new-trajectory-point) - (dolist (j (send robot :joint-list)) - ;; for continuous rotational joint - (when (and (> (- (send j :max-angle) (send j :min-angle)) 360) - (> (abs (elt diff-av i)) 180)) - (ros::ros-warn "continuous joint (~A) moves ~A degree, commanded joint differs from original trajectory to avoid unintentional 360 rotation" (send j :name) (elt diff-av i)) - (setq add-new-trajectory-point t)) - (incf i (send j :joint-dof))) - add-new-trajectory-point)) - (:angle-vector-raw (av &optional (tm 3000) &rest args) - (let* ((prev-av (send self :state :potentio-vector :wait-until-update t)) - (diff-av (v- av prev-av))) - (when (send self :check-continuous-joint-move-over-180 diff-av) - (let* (avs (minjerk (instance minjerk-interpolator :init)) - (scale-av (send self :sub-angle-vector av prev-av)) - dist div) - (setq dist (abs (geo::find-extream (coerce diff-av cons) #'abs #'>=))) - (setq div (round (/ dist 120.0))) - (send minjerk :reset - :position-list (list prev-av (v+ prev-av scale-av)) - :time-list (list tm)) - (send minjerk :start-interpolation) - (send minjerk :pass-time (/ tm div)) - (dotimes (i div) - (setq avs (append avs (list (send minjerk :pass-time (/ tm div)))))) - (send* self :angle-vector-sequence-raw avs (make-list div :initial-element (/ tm div)) args) - (return-from :angle-vector-raw (car (last avs))))) - (send-super* :angle-vector av tm args))) - (:angle-vector-sequence-raw (&rest args) (send-super* :angle-vector-sequence args)) - (:angle-vector - (av &optional (tm 3000) &rest args) ;; (ctype controller-type) (start-time 0) &rest args - ;; &key (clear-velocities t) &allow-other-keys) - "Send joind angle to robot with self-collision motion planning, this method retuns immediately, so use :wait-interpolation to block until the motion stops. -- av : joint angle vector [rad] -- tm : time to goal in [msec] -" - (let ((ctype controller-type) (start-time 0) (start-offset-time nil) (clear-velocities t)) - ;; as of 0.3.x, :angle-vector (robot-interface) :acceps tm ctype start-time as optional arguments, but in here we prefer old API - (if (= (length args) 1) ;; args must be ctype - (setq ctype (car args) - args (cdr args))) - (if (and (>= (length args) 2) (null (member (car args) '(:start-time :clear-velocities))));; args must be ctype start-time - (setq ctype (car args) - start-time (cadr args) - args (cddr args))) - (if (member :start-time args) (setq start-time (cadr (member :start-time args)))) - (if (member :start-offset-time args) (setq start-offset-time (cadr (member :start-offset-time args)))) - (if (member :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args)))) - ;; for simulation mode - (when (and (not (boundp '*co*)) (send self :simulation-modep)) - (return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args))) - ;; - (when (not (numberp tm)) - (ros::warn ":angle-vector tm is not a number, use :angle-vector av tm args")) - (send* self :angle-vector-motion-plan av :ctype ctype :move-arm :rarm :total-time tm - :start-offset-time (if start-offset-time start-offset-time start-time) - :clear-velocities clear-velocities args))) - (:angle-vector-sequence - (avs &optional tms &rest args) ;; (ctype controller-type) (start-time 0) &rest args - ;; &key (clear-velocities t) &allow-other-keys) - "Send joind angle to robot with self-collision motion planning, this method returns immediately, so use :wait-interpolation to block until the motion stops. -- avs : sequence of joint angle vector [rad] -- tms : list of time to goal from previous angle-vector point in [msec] -" - (let ((ctype controller-type) (start-time 0) (start-offset-time nil) (clear-velocities t)) - ;; as of 0.3.x, :angle-vector (robot-interface) :acceps tm ctype start-time as optional arguments, but in here we prefer old API - (if (= (length args) 1) ;; args must be ctype - (setq ctype (car args) - args (cdr args))) - (if (and (>= (length args) 2) (null (member (car args) '(:start-time :clear-velocities))));; args must be ctype start-time - (setq ctype (car args) - start-time (cadr args) - args (cddr args))) - (if (member :start-offset-time args) (setq start-offset-time (cadr (member :start-offset-time args)))) - (if (member :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args)))) - ;; for simulation mode - (when (send self :simulation-modep) - (return-from :angle-vector-sequence - (send* self :angle-vector-sequence-raw avs tms ctype start-time args))) - (unless (and (listp tms) (every #'numberp tms)) - (ros::warn ":angle-vector-sequence tms is not a list of number, use :angle-vector-sequence av tms args")) - (if tms - (setq tms (apply #'+ tms)) - (setq tms 3000)) - (send* self :angle-vector-motion-plan avs :ctype ctype :move-arm :rarm :total-time tms - :start-offset-time (if start-offset-time start-offset-time start-time) - :clear-velocities clear-velocities 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 '*co*) - (ros::ros-warn ";; collision-object-publisher wait for \"my_gen3/apply_planning_scene\" service for ~A sec~%" 5) - (if (ros::wait-for-service "my_gen3/apply_planning_scene" 5) - (setq *co* (instance collision-object-publisher - :init - :service-name "my_gen3/apply_planning_scene" - :scene-service "my_gen3/get_planning_scene")) - (ros::ros-warn ";; could not find \"my_gen3/apply_planning_scene\" skip creating *co*~%"))) - (unless (boundp '*ri*) (setq *ri* (instance gen3-interface :init))) - - (ros::spin-once) - (send *ri* :spin-once) - - (when create-viewer (objects (list *gen3*))) - ) - -(defclass gen3-moveit-environment - :super moveit-environment) -(defmethod gen3-moveit-environment - (:init (&key ((:robot rb) *gen3*) &rest args) - (send-super* :init - :robot rb - :frame-id "base_link" - :scene-service "my_gen3/get_planning_scene" - :planning-service "my_gen3/plan_kinematic_path" - :execute-service "my_gen3/execute_kinematic_path" - :query-planner-interface-service "my_gen3/query_planner_interface" - :planning-scene-world "my_gen3/planning_scene_world" - :state-validity-service "my_gen3/check_state_validity" - args)) - (:default-configuration () - (list (list :rarm - (cons :group-name "arm") - (cons :target-link - (send self :search-link-from-name "end_effector_link")) - (cons :joint-list (send robot :arm :joint-list))))) - ) diff --git a/jsk_kinova_robot/kinovaeus/gen3-lite-interface.l b/jsk_kinova_robot/kinovaeus/gen3-lite-interface.l deleted file mode 100644 index 0850a2ab60..0000000000 --- a/jsk_kinova_robot/kinovaeus/gen3-lite-interface.l +++ /dev/null @@ -1,191 +0,0 @@ -(require "package://kinovaeus/gen3-lite-utils.l") -(require "package://pr2eus/robot-interface.l") -(require "package://pr2eus_moveit/euslisp/robot-moveit.l") - -(defclass gen3-lite-interface - :super robot-interface - :slots (gripper-action moveit-robot) - ) - -(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 :arm-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)) - (setq moveit-robot (instance gen3_lite_gen3_lite_2f-robot :init)) - (send self :set-moveit-environment (instance gen3-lite-moveit-environment :init :robot moveit-robot)) - ) - (:default-controller () (send self :arm-controller)) - (:arm-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) - (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.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))))) - (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.05 args)) - (:stop-grasp - (&rest args &key &allow-other-keys) - (send* self :go-grasp :pos 0.95 args)) - ;; :angle-vector-xxx and :check-continuous-joint-move-over-180 - ;; are mainly copied from package://fetcheus/fetch-interface.l - (:check-continuous-joint-move-over-180 ;; can be removed if http//github.com/jsk-ros-pkg/jsk_pr2eus/pull/322 merged - (diff-av) - (let ((i 0) add-new-trajectory-point) - (dolist (j (send robot :joint-list)) - ;; for continuous rotational joint - (when (and (> (- (send j :max-angle) (send j :min-angle)) 360) - (> (abs (elt diff-av i)) 180)) - (ros::ros-warn "continuous joint (~A) moves ~A degree, commanded joint differs from original trajectory to avoid unintentional 360 rotation" (send j :name) (elt diff-av i)) - (setq add-new-trajectory-point t)) - (incf i (send j :joint-dof))) - add-new-trajectory-point)) - (:angle-vector-raw (av &optional (tm 3000) &rest args) - (let* ((prev-av (send self :state :potentio-vector :wait-until-update t)) - (diff-av (v- av prev-av))) - (when (send self :check-continuous-joint-move-over-180 diff-av) - (let* (avs (minjerk (instance minjerk-interpolator :init)) - (scale-av (send self :sub-angle-vector av prev-av)) - dist div) - (setq dist (abs (geo::find-extream (coerce diff-av cons) #'abs #'>=))) - (setq div (round (/ dist 120.0))) - (send minjerk :reset - :position-list (list prev-av (v+ prev-av scale-av)) - :time-list (list tm)) - (send minjerk :start-interpolation) - (send minjerk :pass-time (/ tm div)) - (dotimes (i div) - (setq avs (append avs (list (send minjerk :pass-time (/ tm div)))))) - (send* self :angle-vector-sequence-raw avs (make-list div :initial-element (/ tm div)) args) - (return-from :angle-vector-raw (car (last avs))))) - (send-super* :angle-vector av tm args))) - (:angle-vector-sequence-raw (&rest args) (send-super* :angle-vector-sequence args)) - (:angle-vector - (av &optional (tm 3000) &rest args) ;; (ctype controller-type) (start-time 0) &rest args - ;; &key (clear-velocities t) &allow-other-keys) - "Send joind angle to robot with self-collision motion planning, this method retuns immediately, so use :wait-interpolation to block until the motion stops. -- av : joint angle vector [rad] -- tm : time to goal in [msec] -" - (let ((ctype controller-type) (start-time 0) (start-offset-time nil) (clear-velocities t)) - ;; as of 0.3.x, :angle-vector (robot-interface) :acceps tm ctype start-time as optional arguments, but in here we prefer old API - (if (= (length args) 1) ;; args must be ctype - (setq ctype (car args) - args (cdr args))) - (if (and (>= (length args) 2) (null (member (car args) '(:start-time :clear-velocities))));; args must be ctype start-time - (setq ctype (car args) - start-time (cadr args) - args (cddr args))) - (if (member :start-time args) (setq start-time (cadr (member :start-time args)))) - (if (member :start-offset-time args) (setq start-offset-time (cadr (member :start-offset-time args)))) - (if (member :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args)))) - ;; for simulation mode - (when (and (not (boundp '*co*)) (send self :simulation-modep)) - (return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args))) - ;; - (when (not (numberp tm)) - (ros::warn ":angle-vector tm is not a number, use :angle-vector av tm args")) - (send* self :angle-vector-motion-plan av :ctype ctype :move-arm :rarm :total-time tm - :start-offset-time (if start-offset-time start-offset-time start-time) - :clear-velocities clear-velocities args))) - (:angle-vector-sequence - (avs &optional tms &rest args) ;; (ctype controller-type) (start-time 0) &rest args - ;; &key (clear-velocities t) &allow-other-keys) - "Send joind angle to robot with self-collision motion planning, this method returns immediately, so use :wait-interpolation to block until the motion stops. -- avs : sequence of joint angle vector [rad] -- tms : list of time to goal from previous angle-vector point in [msec] -" - (let ((ctype controller-type) (start-time 0) (start-offset-time nil) (clear-velocities t)) - ;; as of 0.3.x, :angle-vector (robot-interface) :acceps tm ctype start-time as optional arguments, but in here we prefer old API - (if (= (length args) 1) ;; args must be ctype - (setq ctype (car args) - args (cdr args))) - (if (and (>= (length args) 2) (null (member (car args) '(:start-time :clear-velocities))));; args must be ctype start-time - (setq ctype (car args) - start-time (cadr args) - args (cddr args))) - (if (member :start-offset-time args) (setq start-offset-time (cadr (member :start-offset-time args)))) - (if (member :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args)))) - ;; for simulation mode - (when (send self :simulation-modep) - (return-from :angle-vector-sequence - (send* self :angle-vector-sequence-raw avs tms ctype start-time args))) - (unless (and (listp tms) (every #'numberp tms)) - (ros::warn ":angle-vector-sequence tms is not a list of number, use :angle-vector-sequence av tms args")) - (if tms - (setq tms (apply #'+ tms)) - (setq tms 3000)) - (send* self :angle-vector-motion-plan avs :ctype ctype :move-arm :rarm :total-time tms - :start-offset-time (if start-offset-time start-offset-time start-time) - :clear-velocities clear-velocities 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 '*co*) - (ros::ros-warn ";; collision-object-publisher wait for \"my_gen3_lite/apply_planning_scene\" service for ~A sec~%" 5) - (if (ros::wait-for-service "my_gen3_lite/apply_planning_scene" 5) - (setq *co* (instance collision-object-publisher - :init - :service-name "my_gen3_lite/apply_planning_scene" - :scene-service "my_gen3_lite/get_planning_scene")) - (ros::ros-warn ";; could not find \"my_gen3_lite/apply_planning_scene\" skip creating *co*~%"))) - (unless (boundp '*ri*) (setq *ri* (instance gen3-lite-interface :init))) - - (ros::spin-once) - (send *ri* :spin-once) - - (when create-viewer (objects (list *gen3-lite*))) - ) - -(defclass gen3-lite-moveit-environment - :super moveit-environment) -(defmethod gen3-lite-moveit-environment - (:init (&key ((:robot rb) *gen3-lite*) &rest args) - (send-super* :init - :robot rb - :frame-id "base_link" - :scene-service "my_gen3_lite/get_planning_scene" - :planning-service "my_gen3_lite/plan_kinematic_path" - :execute-service "my_gen3_lite/execute_kinematic_path" - :query-planner-interface-service "my_gen3_lite/query_planner_interface" - :planning-scene-world "my_gen3_lite/planning_scene_world" - :state-validity-service "my_gen3_lite/check_state_validity" - args)) - (:default-configuration () - (list (list :rarm - (cons :group-name "arm") - (cons :target-link - (send self :search-link-from-name "end_effector_link")) - (cons :joint-list (send robot :arm :joint-list))))) - ) diff --git a/jsk_kinova_robot/kinovaeus/gen3-lite-utils.l b/jsk_kinova_robot/kinovaeus/gen3-lite-utils.l deleted file mode 100644 index 30b7045132..0000000000 --- a/jsk_kinova_robot/kinovaeus/gen3-lite-utils.l +++ /dev/null @@ -1,20 +0,0 @@ -(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 - (: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)) - ) diff --git a/jsk_kinova_robot/kinovaeus/gen3-lite.yaml b/jsk_kinova_robot/kinovaeus/gen3-lite.yaml deleted file mode 100644 index 9e2f76a477..0000000000 --- a/jsk_kinova_robot/kinovaeus/gen3-lite.yaml +++ /dev/null @@ -1,20 +0,0 @@ -## -## - 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 deleted file mode 100644 index c17d5c33d1..0000000000 --- a/jsk_kinova_robot/kinovaeus/gen3-utils.l +++ /dev/null @@ -1,19 +0,0 @@ -(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 - (: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)) - ) diff --git a/jsk_kinova_robot/kinovaeus/gen3.yaml b/jsk_kinova_robot/kinovaeus/gen3.yaml deleted file mode 100644 index 8aeec5b5cb..0000000000 --- a/jsk_kinova_robot/kinovaeus/gen3.yaml +++ /dev/null @@ -1,21 +0,0 @@ -## -## - 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/gen3_lite_gen3_lite_2f.yaml b/jsk_kinova_robot/kinovaeus/gen3_lite_gen3_lite_2f.yaml index b22a9e1494..dee7590b69 100644 --- a/jsk_kinova_robot/kinovaeus/gen3_lite_gen3_lite_2f.yaml +++ b/jsk_kinova_robot/kinovaeus/gen3_lite_gen3_lite_2f.yaml @@ -10,6 +10,8 @@ rarm: - joint_5 : rarm-elbow-y - joint_6 : rarm-wrist-p +# reset-pose is the home pose +# https://github.com/Kinovarobotics/ros_kortex/blob/330c55bce8c3d463cca2492b3e0c89204f235640/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf.xacro#L31-L38 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] diff --git a/jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_140.yaml b/jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_140.yaml index c340947654..d06767b91f 100644 --- a/jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_140.yaml +++ b/jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_140.yaml @@ -11,9 +11,11 @@ rarm: - joint_6 : rarm-wrist-p - joint_7 : rarm-wrist-y +# reset-pose is the home pose +# https://github.com/Kinovarobotics/ros_kortex/blob/330c55bce8c3d463cca2492b3e0c89204f235640/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf.xacro#L38-L46 angle-vector: init-pose : [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - reset-pose : [0.0, -20.0, 180.0, -140.0, 0.0, 30.0, 90.0] + reset-pose : [0.0, 15.0, 180.0, -130.0, 0.0, 55.0, 90.0] rarm-end-coords: parent : tool_frame diff --git a/jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_85.yaml b/jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_85.yaml index c340947654..2c4b9ad92d 100644 --- a/jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_85.yaml +++ b/jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_85.yaml @@ -11,9 +11,11 @@ rarm: - joint_6 : rarm-wrist-p - joint_7 : rarm-wrist-y +# reset-pose is the home pose +# https://github.com/Kinovarobotics/ros_kortex/blob/330c55bce8c3d463cca2492b3e0c89204f235640/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf.xacro#L38-L46 angle-vector: init-pose : [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - reset-pose : [0.0, -20.0, 180.0, -140.0, 0.0, 30.0, 90.0] + reset-pose : [0.0, 15.0, 180.0, -130.0, 0.0, 55.0, 90.0] rarm-end-coords: parent : tool_frame diff --git a/jsk_kinova_robot/kinovaeus/kinova-interface.l b/jsk_kinova_robot/kinovaeus/kinova-interface.l index 6e1c98f4ec..f4b724e406 100644 --- a/jsk_kinova_robot/kinovaeus/kinova-interface.l +++ b/jsk_kinova_robot/kinovaeus/kinova-interface.l @@ -1,5 +1,6 @@ (require "package://kinovaeus/kinova-utils.l") (require "package://pr2eus/robot-interface.l") +(require "package://pr2eus_moveit/euslisp/robot-moveit.l") (if (not (find-package "kinova")) (make-package "kinova")) @@ -22,7 +23,7 @@ "/arm_gen3/robotiq_2f_140_gripper_controller/gripper_cmd") ((derivedp robot gen3_robotiq_2f_85-robot) "/arm_gen3/robotiq_2f_85_gripper_controller/gripper_cmd") - ((derivedp robot gen3_robotiq_2f_140-robot) + ((derivedp robot gen3_lite_gen3_lite_2f-robot) "/arm_gen3/gen3_lite_2f_gripper_gripper_controller/gripper_cmd") (t (warning-message 1 "unknown kinova robot type ~A~%" robot) @@ -30,17 +31,25 @@ control_msgs::GripperCommandAction :groupname groupname)) (setq moveit-robot (instance (eval (send (class robot) :name)) :init)) - (send self :set-moveit-environment (instance gen3-moveit-environment :init :robot moveit-robot)) + (send self :set-moveit-environment (instance kinova-moveit-environment :init :robot moveit-robot)) self))) ;; default robot-interface methods (:default-controller () (send self :arm-controller)) (:arm-controller () - (list (list - (cons :controller-action "arm_gen3/gen3_joint_trajectory_controller/follow_joint_trajectory") - (cons :controller-state "arm_gen3/gen3_joint_trajectory_controller/state") ;; @TODO: This is dummy topoic name. The kinova_driver did not publish JointTrajectoryControllerState - (cons :action-type control_msgs::FollowJointTrajectoryAction) - (cons :joint-names (send-all (send robot :joint-list) :name))))) + (append + (cond ((or (derivedp robot gen3_robotiq_2f_140-robot) (derivedp robot gen3_robotiq_2f_85-robot)) + (list + (cons :controller-action + "arm_gen3/gen3_joint_trajectory_controller/follow_joint_trajectory") + (cons :controller-state "arm_gen3/gen3_joint_trajectory_controller/state"))) ;; @TODO: This is dummy topoic name. The kinova_driver did not publish JointTrajectoryControllerState + ((derivedp robot gen3_lite_gen3_lite_2f-robot) + (list + (cons :controller-action + "arm_gen3/gen3_lite_joint_trajectory_controller/follow_joint_trajectory") + (cons :controller-state "arm_gen3/gen3_lite_joint_trajectory_controller/state")))) ;; @TODO: This is dummy topoic name. The kinova_driver did not publish JointTrajectoryControllerState + (list (cons :action-type control_msgs::FollowJointTrajectoryAction) + (cons :joint-names (send-all (send robot :joint-list) :name)))))) (:go-grasp (&rest args) (cond ((derivedp robot gen3_robotiq_2f_140-robot) From f80148ca976ce15ec466937d4c78efff1638cebf Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Mon, 4 Jan 2021 19:24:14 +0900 Subject: [PATCH 17/41] Add grasp methods document --- jsk_kinova_robot/README.md | 8 +++++++- jsk_kinova_robot/kinovaeus/kinova-interface.l | 5 +++++ jsk_kinova_robot/kinovaeus/kinova.l | 7 ++++++- 3 files changed, 18 insertions(+), 2 deletions(-) diff --git a/jsk_kinova_robot/README.md b/jsk_kinova_robot/README.md index 7667eb7f6b..4354e8f9ce 100644 --- a/jsk_kinova_robot/README.md +++ b/jsk_kinova_robot/README.md @@ -94,6 +94,12 @@ You can also use move-end-rot method to turn the gripper. (send *kinova* :arm :move-end-rot -90 :z) ``` +To open/close gripper, you can use `start-grasp` and `stop-grasp` method. +``` +(send *gen3* :start-grasp) +(send *gen3* :stop-grasp) +``` + To control real robot. you can use *ri* object. ``` (send *ri* :angle-vector (send *kinova* :angle-vector) 2000) @@ -111,7 +117,7 @@ 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`. +To open and close the gripper, You can use `:start-grasp` and `:stop-grasp`. ``` (send *ri* :stop-grasp) diff --git a/jsk_kinova_robot/kinovaeus/kinova-interface.l b/jsk_kinova_robot/kinovaeus/kinova-interface.l index f4b724e406..6a45157fd8 100644 --- a/jsk_kinova_robot/kinovaeus/kinova-interface.l +++ b/jsk_kinova_robot/kinovaeus/kinova-interface.l @@ -83,6 +83,9 @@ ;; grasp for robotiq (:go-grasp-robotiq (&key (pos 0.0) (wait t)) (when (send self :simulation-modep) + ;; mimic joint + ;; https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_arg2f_85_model_macro.xacro + ;; https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_arg2f_140_model_macro.xacro (let ((pos-deg (rad2deg pos))) (send robot :finger_joint :joint-angle pos-deg) (send robot :left_inner_finger_joint :joint-angle (* -1 pos-deg)) @@ -111,6 +114,8 @@ ;; grasp for gen3 lite (:go-grasp-lite (&key (pos 0.0) (wait t)) (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 (rad2deg pos)) (send robot :right_finger_tip_joint :joint-angle diff --git a/jsk_kinova_robot/kinovaeus/kinova.l b/jsk_kinova_robot/kinovaeus/kinova.l index f7274182bf..f6502d423d 100644 --- a/jsk_kinova_robot/kinovaeus/kinova.l +++ b/jsk_kinova_robot/kinovaeus/kinova.l @@ -18,6 +18,8 @@ (defmethod gen3_robotiq_2f_85-robot (:arm (&rest args) (send* self :rarm args)) ;; enable to call send *kinova* :arm :angle-vector (:go-grasp (&key (pos 0)) + ;; mimic joint + ;; https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_arg2f_85_model_macro.xacro (let ((pos-deg (rad2deg pos))) (send self :finger_joint :joint-angle pos-deg) (send self :left_inner_finger_joint :joint-angle (* -1 pos-deg)) @@ -33,6 +35,8 @@ (defmethod gen3_robotiq_2f_140-robot (:arm (&rest args) (send* self :rarm args)) ;; enable to call send *kinova* :arm :angle-vector (:go-grasp (&key (pos 0)) + ;; mimic joint + ;; https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_arg2f_140_model_macro.xacro (let ((pos-deg (rad2deg pos))) (send self :finger_joint :joint-angle pos-deg) (send self :left_inner_finger_joint :joint-angle (* -1 pos-deg)) @@ -48,6 +52,8 @@ (defmethod gen3_lite_gen3_lite_2f-robot (:arm (&rest args) (send* self :rarm args)) ;; enable to call send *kinova* :arm :angle-vector (: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 (rad2deg pos)) (send self :right_finger_tip_joint :joint-angle @@ -61,4 +67,3 @@ (:stop-grasp () (send self :go-grasp :pos 0.95)) ) - From 8b1574635ef61d84c977731aea54eb7aa4745f00 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Mon, 4 Jan 2021 21:22:12 +0900 Subject: [PATCH 18/41] Fix grasp functions --- jsk_kinova_robot/README.md | 21 ++---- jsk_kinova_robot/kinovaeus/kinova-interface.l | 71 +++++++++++++------ jsk_kinova_robot/kinovaeus/kinova.l | 12 ++-- 3 files changed, 63 insertions(+), 41 deletions(-) diff --git a/jsk_kinova_robot/README.md b/jsk_kinova_robot/README.md index 4354e8f9ce..0e1d677857 100644 --- a/jsk_kinova_robot/README.md +++ b/jsk_kinova_robot/README.md @@ -51,29 +51,22 @@ roslaunch jsk_kinova_startup kinova_bringup.launch Start rviz from different terminal ```bash -roslaunch jsk_kinova_startup rvi.launch +roslaunch jsk_kinova_startup rviz.launch ``` ## Use EusLisp model To control the robot form EusLisp. Please start `roseus` and type as follows. ``` -;; Gen3 + (load "package://kinovaeus/kinova-interface.l") -(kinova-init) +;; Gen3 with robotiq 2f 85 gripper +(kinova-init :type :gen3_robotiq_2f_85) +;; Gen3 with robotiq 2f 140 gripper +(kinova-init :type :gen3_robotiq_2f_140) ;; Gen3 Lite -(load "package://kinovaeus/kinova-lite-interface.l") (kinova-init :type :gen3_lite_gen3_lite_2f) ``` -## Use EusLisp model - -To control the robot form EusLisp. Please start `roseus` and type as follows. - -``` -(load "package://kinovaeus/kinova-interface.l") -(kinova-init) -``` - Use `:angle-vector` method to specify the arm joint angle. ``` (send *kinova* :angle-vector #f(0.0 15.0 180.0 -130.0 0.0 55.0 90.0)) @@ -81,7 +74,7 @@ Use `:angle-vector` method to specify the arm joint angle. You can also use `:inverse-kinematics` method to specify the arm pose from target coordinates. ``` -(send *kinova* :arm :inverse-kinematics (make-coords :pos #f(300 0 200) :rpy (float-vector pi 0 pi)) :debug-view t) +(send *ri* :angle-vector (send *kinova* :arm :inverse-kinematics (make-coords :pos #f(300 0 200) :rpy (float-vector 0 pi/2 0)) :debug-view t) 3000) ``` To move the gripper 50 [mm] up, you can use `move-end-pos` method. diff --git a/jsk_kinova_robot/kinovaeus/kinova-interface.l b/jsk_kinova_robot/kinovaeus/kinova-interface.l index 6a45157fd8..a3b6e0ffbd 100644 --- a/jsk_kinova_robot/kinovaeus/kinova-interface.l +++ b/jsk_kinova_robot/kinovaeus/kinova-interface.l @@ -24,7 +24,7 @@ ((derivedp robot gen3_robotiq_2f_85-robot) "/arm_gen3/robotiq_2f_85_gripper_controller/gripper_cmd") ((derivedp robot gen3_lite_gen3_lite_2f-robot) - "/arm_gen3/gen3_lite_2f_gripper_gripper_controller/gripper_cmd") + "/arm_gen3/gen3_lite_2f_gripper_controller/gripper_cmd") (t (warning-message 1 "unknown kinova robot type ~A~%" robot) )) @@ -53,39 +53,38 @@ (:go-grasp (&rest args) (cond ((derivedp robot gen3_robotiq_2f_140-robot) - (send* self :go-grasp-robotiq args)) + (send* self :go-grasp-robotiq-2f-140 args)) ((derivedp robot gen3_robotiq_2f_85-robot) - (send* self :go-grasp-robotiq args)) - ((derivedp robot gen3_robotiq_2f_140-robot) - (send* self :go-grasp-gen3-lite args)) + (send* self :go-grasp-robotiq-2f-85 args)) + ((derivedp robot gen3_lite_gen3_lite_2f-robot) + (send* self :go-grasp-lite args)) (t (warning-message 1 "unknown kinova robot type ~A~%" robot)))) (:start-grasp (&rest args) (cond ((derivedp robot gen3_robotiq_2f_140-robot) - (send* self :start-grasp-robotiq args)) + (send* self :start-grasp-robotiq-2f-140 args)) ((derivedp robot gen3_robotiq_2f_85-robot) - (send* self :start-grasp-robotiq args)) - ((derivedp robot gen3_robotiq_2f_140-robot) - (send* self :start-grasp-gen3-lite args)) + (send* self :start-grasp-robotiq-2f-85 args)) + ((derivedp robot gen3_lite_gen3_lite_2f-robot) + (send* self :start-grasp-lite args)) (t (warning-message 1 "unknown kinova robot type ~A~%" robot)))) (:stop-grasp (&rest args) (cond ((derivedp robot gen3_robotiq_2f_140-robot) - (send* self :stop-grasp-robotiq args)) + (send* self :stop-grasp-robotiq-2f-140 args)) ((derivedp robot gen3_robotiq_2f_85-robot) - (send* self :stop-grasp-robotiq args)) - ((derivedp robot gen3_robotiq_2f_140-robot) - (send* self :stop-grasp-gen3-lite args)) + (send* self :stop-grasp-robotiq-2f-85 args)) + ((derivedp robot gen3_lite_gen3_lite_2f-robot) + (send* self :stop-grasp-lite args)) (t (warning-message 1 "unknown kinova robot type ~A~%" robot)))) - ;; grasp for robotiq - (:go-grasp-robotiq (&key (pos 0.0) (wait t)) + ;; grasp for robotiq 2f 85 + (:go-grasp-robotiq-2f-85 (&key (pos 0.0) (wait t)) (when (send self :simulation-modep) ;; mimic joint ;; https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_arg2f_85_model_macro.xacro - ;; https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_arg2f_140_model_macro.xacro (let ((pos-deg (rad2deg pos))) (send robot :finger_joint :joint-angle pos-deg) (send robot :left_inner_finger_joint :joint-angle (* -1 pos-deg)) @@ -93,7 +92,7 @@ (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-robotiq t)) + (return-from :go-grasp-robotiq-2f-85 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)) @@ -105,12 +104,42 @@ (when wait (send gripper-action :wait-for-result)) (setq result (send gripper-action :get-result)) result)) - (:start-grasp-robotiq + (:start-grasp-robotiq-2f-85 + (&rest args &key &allow-other-keys) + (send* self :go-grasp-robotiq-2f-85 :pos 0.8 args)) + (:stop-grasp-robotiq-2f-85 + (&rest args &key &allow-other-keys) + (send* self :go-grasp-robotiq-2f-85 :pos 0.0 args)) + ;; grasp for robotiq 2f 140 + (:go-grasp-robotiq-2f-140 (&key (pos 0.0) (wait t)) + (when (send self :simulation-modep) + ;; mimic joint + ;; https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_arg2f_140_model_macro.xacro + (let ((pos-deg (rad2deg pos))) + (send self :finger_joint :joint-angle pos-deg) + (send self :left_inner_finger_joint :joint-angle pos-deg) + (send self :left_inner_knuckle_joint :joint-angle (* -1 pos-deg)) + (send self :right_inner_finger_joint :joint-angle pos-deg) + (send self :right_inner_knuckle_joint :joint-angle (* -1 pos-deg)) + (send self :right_outer_knuckle_joint :joint-angle (* -1 pos-deg))) + (return-from :go-grasp-robotiq-2f-140 t)) + (let ((pos-max 0.69) (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-robotiq-2f-140 (&rest args &key &allow-other-keys) - (send* self :go-grasp-robotiq :pos 0.8 args)) - (:stop-grasp-robotiq + (send* self :go-grasp-robotiq-2f-140 :pos 0.69 args)) + (:stop-grasp-robotiq-2f-140 (&rest args &key &allow-other-keys) - (send* self :go-grasp-robotiq :pos 0.0 args)) + (send* self :go-grasp-robotiq-2f-140 :pos 0.0 args)) ;; grasp for gen3 lite (:go-grasp-lite (&key (pos 0.0) (wait t)) (when (send self :simulation-modep) diff --git a/jsk_kinova_robot/kinovaeus/kinova.l b/jsk_kinova_robot/kinovaeus/kinova.l index f6502d423d..5efa06c698 100644 --- a/jsk_kinova_robot/kinovaeus/kinova.l +++ b/jsk_kinova_robot/kinovaeus/kinova.l @@ -39,13 +39,13 @@ ;; https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_arg2f_140_model_macro.xacro (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))) + (send self :left_inner_finger_joint :joint-angle pos-deg) + (send self :left_inner_knuckle_joint :joint-angle (* -1 pos-deg)) + (send self :right_inner_finger_joint :joint-angle pos-deg) + (send self :right_inner_knuckle_joint :joint-angle (* -1 pos-deg)) + (send self :right_outer_knuckle_joint :joint-angle (* -1 pos-deg)))) (:start-grasp () - (send self :go-grasp :pos 0.8)) + (send self :go-grasp :pos 0.69)) (:stop-grasp () (send self :go-grasp :pos 0.0)) ) From a569e75262a2f7e2c5e558b86b69e547fcc7e4d0 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Mon, 4 Jan 2021 22:34:22 +0900 Subject: [PATCH 19/41] Fix tests --- jsk_kinova_robot/kinovaeus/CMakeLists.txt | 8 +- .../kinovaeus/test/test-gen3-lite-moveit.l | 31 ------- .../kinovaeus/test/test-gen3-lite-moveit.test | 13 --- .../kinovaeus/test/test-gen3-lite.l | 28 ------- .../kinovaeus/test/test-gen3-lite.test | 3 - .../kinovaeus/test/test-gen3-moveit.l | 31 ------- jsk_kinova_robot/kinovaeus/test/test-gen3.l | 28 ------- .../kinovaeus/test/test-gen3.test | 3 - .../test/test-kinova-kinematic-simulator.l | 80 +++++++++++++++++++ .../test/test-kinova-kinematic-simulator.test | 3 + .../kinovaeus/test/test-kinova-moveit.l | 80 +++++++++++++++++++ ...n3-moveit.test => test-kinova-moveit.test} | 3 +- jsk_kinova_robot/kinovaeus/test/test-kinova.l | 5 +- 13 files changed, 169 insertions(+), 147 deletions(-) delete mode 100755 jsk_kinova_robot/kinovaeus/test/test-gen3-lite-moveit.l delete mode 100644 jsk_kinova_robot/kinovaeus/test/test-gen3-lite-moveit.test delete mode 100755 jsk_kinova_robot/kinovaeus/test/test-gen3-lite.l delete mode 100644 jsk_kinova_robot/kinovaeus/test/test-gen3-lite.test delete mode 100755 jsk_kinova_robot/kinovaeus/test/test-gen3-moveit.l delete mode 100755 jsk_kinova_robot/kinovaeus/test/test-gen3.l delete mode 100644 jsk_kinova_robot/kinovaeus/test/test-gen3.test create mode 100755 jsk_kinova_robot/kinovaeus/test/test-kinova-kinematic-simulator.l create mode 100644 jsk_kinova_robot/kinovaeus/test/test-kinova-kinematic-simulator.test create mode 100755 jsk_kinova_robot/kinovaeus/test/test-kinova-moveit.l rename jsk_kinova_robot/kinovaeus/test/{test-gen3-moveit.test => test-kinova-moveit.test} (76%) diff --git a/jsk_kinova_robot/kinovaeus/CMakeLists.txt b/jsk_kinova_robot/kinovaeus/CMakeLists.txt index 25766ab059..617409507f 100644 --- a/jsk_kinova_robot/kinovaeus/CMakeLists.txt +++ b/jsk_kinova_robot/kinovaeus/CMakeLists.txt @@ -58,11 +58,7 @@ if(CATKIN_ENABLE_TESTING) # Test only roseus add_rostest(test/test-kinova.test) # Test with Kinematic Simulator - add_rostest(test/test-gen3.test) - add_rostest(test/test-gen3-lite.test) + add_rostest(test/test-kinova-kinematic-simulator.test) # Test with Gazebo and MoveIt! - add_rostest(test/test-gen3-moveit.test) - add_rostest(test/test-gen3-lite-moveit.test) + add_rostest(test/test-kinova-moveit.test) endif() - - diff --git a/jsk_kinova_robot/kinovaeus/test/test-gen3-lite-moveit.l b/jsk_kinova_robot/kinovaeus/test/test-gen3-lite-moveit.l deleted file mode 100755 index c0a567c597..0000000000 --- a/jsk_kinova_robot/kinovaeus/test/test-gen3-lite-moveit.l +++ /dev/null @@ -1,31 +0,0 @@ -#!/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-moveit - - (send *gen3-lite* :reset-pose) - (assert (send *ri* :angle-vector (send *gen3-lite* :angle-vector))) - (send *ri* :wait-interpolation) - - (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-raw (send *gen3-lite* :angle-vector))) - (send *ri* :wait-interpolation) - - (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* :wait-interpolation) - - (send *ri* :stop-grasp :wait t) - ) - - -(run-all-tests) -(exit) diff --git a/jsk_kinova_robot/kinovaeus/test/test-gen3-lite-moveit.test b/jsk_kinova_robot/kinovaeus/test/test-gen3-lite-moveit.test deleted file mode 100644 index 3e1a40ba2a..0000000000 --- a/jsk_kinova_robot/kinovaeus/test/test-gen3-lite-moveit.test +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - diff --git a/jsk_kinova_robot/kinovaeus/test/test-gen3-lite.l b/jsk_kinova_robot/kinovaeus/test/test-gen3-lite.l deleted file mode 100755 index 43c008260f..0000000000 --- a/jsk_kinova_robot/kinovaeus/test/test-gen3-lite.l +++ /dev/null @@ -1,28 +0,0 @@ -#!/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 deleted file mode 100644 index d1c2845d30..0000000000 --- a/jsk_kinova_robot/kinovaeus/test/test-gen3-lite.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/jsk_kinova_robot/kinovaeus/test/test-gen3-moveit.l b/jsk_kinova_robot/kinovaeus/test/test-gen3-moveit.l deleted file mode 100755 index 55944759d1..0000000000 --- a/jsk_kinova_robot/kinovaeus/test/test-gen3-moveit.l +++ /dev/null @@ -1,31 +0,0 @@ -#!/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-moveit - - (send *gen3* :reset-pose) - (assert (send *ri* :angle-vector (send *gen3* :angle-vector))) - (send *ri* :wait-interpolation) - - (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-raw (send *gen3* :angle-vector))) - (send *ri* :wait-interpolation) - - (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* :wait-interpolation) - - (send *ri* :stop-grasp :wait t) - ) - - -(run-all-tests) -(exit) diff --git a/jsk_kinova_robot/kinovaeus/test/test-gen3.l b/jsk_kinova_robot/kinovaeus/test/test-gen3.l deleted file mode 100755 index 170b35d175..0000000000 --- a/jsk_kinova_robot/kinovaeus/test/test-gen3.l +++ /dev/null @@ -1,28 +0,0 @@ -#!/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 deleted file mode 100644 index a86e827651..0000000000 --- a/jsk_kinova_robot/kinovaeus/test/test-gen3.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/jsk_kinova_robot/kinovaeus/test/test-kinova-kinematic-simulator.l b/jsk_kinova_robot/kinovaeus/test/test-kinova-kinematic-simulator.l new file mode 100755 index 0000000000..47babc9083 --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/test/test-kinova-kinematic-simulator.l @@ -0,0 +1,80 @@ +#!/usr/bin/env roseus + +(require :unittest "lib/llib/unittest.l") +(init-unit-test) + +;; This test is mainly copied from +;; https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_fetch_robot/fetcheus/test/test-fetcheus.l + +(load "package://kinovaeus/kinova-interface.l") + +(deftest kinova-kinematic-simulator-test + (dolist (robot-type (list :gen3_robotiq_2f_85 :gen3_robotiq_2f_140 :gen3_lite_gen3_lite_2f)) + (let ((ri (instance kinova-interface :init :type robot-type)) + (kinova (kinova :type robot-type)) + diff-av) + ;; + (format t "check reset-pose") + (send kinova :reset-pose) + (send ri :robot :reset-pose) + (send ri :angle-vector (send kinova :angle-vector)) + (send ri :wait-interpolation) + ;; do not care 360 rotaiton + (setq diff-av (send ri :sub-angle-vector (send ri :state :potentio-vector) (send kinova :angle-vector))) + (format t ":state ~A, :robot ~A, diff ~A, ~A~%" (send ri :robot :rarm :wrist-y :joint-angle) (send kinova :rarm :wrist-y :joint-angle) (norm diff-av) (eps= (norm diff-av) 0.0 10.0)) + (assert (eps= (norm diff-av) 0.0 10.0) (format nil ":reset-pose, diff-av ~A" diff-av)) + + (format t "check :ik #f(300 0 200), do not care about 360 degree rotation") + (send kinova :arm :inverse-kinematics (make-coords :pos #f(300 0 200) :rpy (float-vector 0 pi/2 0))) + (send ri :angle-vector (send kinova :angle-vector)) + (send ri :wait-interpolation) + ;; do not care 360 rotaiton + (setq diff-av (send ri :sub-angle-vector (send ri :state :potentio-vector) (send kinova :angle-vector))) + (format t ":state ~A, :robot ~A, diff ~A, ~A~%" (send ri :robot :rarm :wrist-y :joint-angle) (send kinova :rarm :wrist-y :joint-angle) (norm diff-av) (eps= (norm diff-av) 0.0 10.0)) + (assert (eps= (norm diff-av) 0.0 10.0) (format nil ":ik 300 0 200, diff-av ~A" diff-av)) + + ;; do care 360 rotation + (format t "check :init-pose") + (send kinova :init-pose) ;; reset + (send ri :robot :init-pose) + (send ri :angle-vector (send kinova :angle-vector)) + (send ri :wait-interpolation) + (setq diff-av (v- (send ri :state :potentio-vector) (send kinova :angle-vector))) + (format t ":state ~A, :robot ~A, diff ~A, ~A~%" (send ri :robot :rarm :wrist-y :joint-angle) (send kinova :rarm :wrist-y :joint-angle) (norm diff-av) (eps= (norm diff-av) 0.0 10.0)) + (assert (eps= (norm diff-av) 0.0 10.0) (format nil ":init-pose, diff-av ~A" diff-av)) + + ;; 150 is 150 + (format t "check :joint-angle 150, this should goes to 150") + (send kinova :rarm :wrist-y :joint-angle 150) + (send ri :angle-vector (send kinova :angle-vector)) + (send ri :wait-interpolation) + (setq diff-av (v- (send ri :state :potentio-vector) (send kinova :angle-vector))) + (format t ":state ~A, :robot ~A, diff ~A, ~A~%" (send ri :robot :rarm :wrist-y :joint-angle) (send kinova :rarm :wrist-y :joint-angle) (norm diff-av) (eps= (norm diff-av) 0.0 10.0)) + (assert (eps= (norm diff-av) 0.0 10.0) (format nil ":joint-angle 150, diff-av ~A" diff-av)) + + (format t "check :joint-angle-sequence 150 0 -150, this should goes to -150") + ;; to send to -150, use :angle-vector-sequence + (send ri :angle-vector-sequence (list + (progn + (send kinova :rarm :wrist-y :joint-angle 150) + (send kinova :angle-vector)) + (progn + (send kinova :rarm :wrist-y :joint-angle 0) + (send kinova :angle-vector)) + (progn + (send kinova :rarm :wrist-y :joint-angle -150) + (send kinova :angle-vector)))) + (send ri :wait-interpolation) + (setq diff-av (v- (send ri :state :potentio-vector) (send kinova :angle-vector))) + (format t ":state ~A, :robot ~A, diff ~A, ~A~%" (send ri :robot :rarm :wrist-y :joint-angle) (send kinova :rarm :wrist-y :joint-angle) (norm diff-av) (eps= (norm diff-av) 0.0 10.0)) + (assert (eps= (norm diff-av) 0.0 10.0) (format nil ":angle-vector-sequence, diff-av ~A" diff-av)) + ))) + +(deftest kinova-init-test + (let () + (kinova-init :type :gen3_robotiq_2f_85) + (kinova-init :type :gen3_robotiq_2f_140) + (kinova-init :type :gen3_lite_gen3_lite_2f))) + +(run-all-tests) +(exit) diff --git a/jsk_kinova_robot/kinovaeus/test/test-kinova-kinematic-simulator.test b/jsk_kinova_robot/kinovaeus/test/test-kinova-kinematic-simulator.test new file mode 100644 index 0000000000..ecd206569d --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/test/test-kinova-kinematic-simulator.test @@ -0,0 +1,3 @@ + + + diff --git a/jsk_kinova_robot/kinovaeus/test/test-kinova-moveit.l b/jsk_kinova_robot/kinovaeus/test/test-kinova-moveit.l new file mode 100755 index 0000000000..6a90adaf58 --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/test/test-kinova-moveit.l @@ -0,0 +1,80 @@ +#!/usr/bin/env roseus + +(require :unittest "lib/llib/unittest.l") +(init-unit-test) + +;; This test is mainly copied from +;; https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_fetch_robot/fetcheus/test/test-fetch-moveit.l + +(load "package://kinovaeus/kinova-interface.l") + +(setq *ri* (instance kinova-interface :init :type :gen3_robotiq_2f_85)) +(setq *kinova* (kinova :type :gen3_robotiq_2f_85)) + +(send *kinova* :reset-pose) + +(deftest test-kinova-moveit () + (let (tm-0 tm-1) + (setq tm-0 (ros::time-now)) + (while (null (send *ri* :angle-vector #f(0 0 0 0 0 0 0)))) + (send *ri* :wait-interpolation) + (setq tm-1 (ros::time-now)) + (ros::ros-info "time for duration ~A" (send (ros::time- tm-1 tm-0) :to-sec)) + (assert (> (send (ros::time- tm-1 tm-0) :to-sec) 3) "collsion avoidance motion is too fast") + + (setq tm-0 (ros::time-now)) + (while (null (send *ri* :angle-vector (send *kinova* :reset-pose) 2000))) + (send *ri* :wait-interpolation) + (setq tm-1 (ros::time-now)) + (ros::ros-info "time for duration ~A" (send (ros::time- tm-1 tm-0) :to-sec)) + (assert (> (send (ros::time- tm-1 tm-0) :to-sec) 3) "collsion avoidance motion is too fast") + + (setq tm-0 (ros::time-now)) + (while (null (send *ri* :angle-vector #f(0 0 0 0 0 0 0)))) + (send *ri* :wait-interpolation) + (setq tm-1 (ros::time-now)) + (ros::ros-info "time for duration ~A" (send (ros::time- tm-1 tm-0) :to-sec)) + (assert (> (send (ros::time- tm-1 tm-0) :to-sec) 3) "collsion avoidance motion is too fast") + + (setq tm-0 (ros::time-now)) + (while (null (send *ri* :angle-vector (send *kinova* :reset-pose) 2000))) + (send *ri* :wait-interpolation) + (setq tm-1 (ros::time-now)) + (ros::ros-info "time for duration ~A" (send (ros::time- tm-1 tm-0) :to-sec)) + (assert (> (send (ros::time- tm-1 tm-0) :to-sec) 3) "collsion avoidance motion is too fast") + )) + +(deftest test-kinova-moveit-angle-vector-start-time () + (let (tm-0 tm-1 tm-diff) + (send *ri* :angle-vector (send *kinova* :reset-pose)) + (send *ri* :wait-interpolation) + (send *kinova* :rarm :move-end-pos #f(50 0 0) :world) + (send *ri* :angle-vector (send *kinova* :angle-vector) 2000 nil 4) + (setq tm-0 (ros::time-now)) + (send *ri* :wait-interpolation) + (setq tm-1 (ros::time-now)) + (setq tm-diff (send (ros::time- tm-1 tm-0) :to-sec)) + (ros::ros-info "time for duration ~A" tm-diff) + (assert (> tm-diff 4.5) (format nil "start-time is ignored. Traj finishes at ~A" tm-diff)) + (assert (< tm-diff 8) (format nil "start-time is considered multiple times. Traj finishes at ~A" tm-diff)) + )) + +(deftest test-kinova-moveit-angle-vector-sequence-start-time () + (let (avs tm-0 tm-1 tm-diff) + (send *ri* :angle-vector (send *kinova* :reset-pose)) + (send *ri* :wait-interpolation) + (push (send *kinova* :rarm :move-end-pos #f(50 0 0) :world) avs) + (push (send *kinova* :rarm :move-end-pos #f(50 0 0) :world) avs) + (setq avs (reverse avs)) + (send *ri* :angle-vector-sequence avs 2000 nil 4) + (setq tm-0 (ros::time-now)) + (send *ri* :wait-interpolation) + (setq tm-1 (ros::time-now)) + (setq tm-diff (send (ros::time- tm-1 tm-0) :to-sec)) + (ros::ros-info "time for duration ~A" tm-diff) + (assert (> tm-diff 4.5) (format nil "start-time is ignored. Traj finishes at ~A" tm-diff)) + (assert (< tm-diff 8) (format nil "start-time is considered multiple times. Traj finishes at ~A" tm-diff)) + )) + +(run-all-tests) +(exit) diff --git a/jsk_kinova_robot/kinovaeus/test/test-gen3-moveit.test b/jsk_kinova_robot/kinovaeus/test/test-kinova-moveit.test similarity index 76% rename from jsk_kinova_robot/kinovaeus/test/test-gen3-moveit.test rename to jsk_kinova_robot/kinovaeus/test/test-kinova-moveit.test index 7442091f34..83026e8907 100644 --- a/jsk_kinova_robot/kinovaeus/test/test-gen3-moveit.test +++ b/jsk_kinova_robot/kinovaeus/test/test-kinova-moveit.test @@ -2,12 +2,13 @@ + - diff --git a/jsk_kinova_robot/kinovaeus/test/test-kinova.l b/jsk_kinova_robot/kinovaeus/test/test-kinova.l index 436b9de644..a63db0dafe 100755 --- a/jsk_kinova_robot/kinovaeus/test/test-kinova.l +++ b/jsk_kinova_robot/kinovaeus/test/test-kinova.l @@ -9,7 +9,7 @@ (let (robot) (setq robot (kinova)) (objects (list robot)) - ;; rotation-angle NIL is returned when rotation-matrix is a unit-matrix + ;; rotation-angle NIL is returned when rotation-matrix is a unit-matrix (warning-message 2 "~A end-corods ~A~%" (send robot :name) (rotation-angle (send robot :arm :end-coords :worldrot))) (assert (eps= (norm (send (make-coords) :difference-rotation (send robot :arm :end-coords))) 0)) @@ -21,10 +21,9 @@ (setq robot (kinova :type :gen3_lite_gen3_lite_2f)) (objects (list robot)) (warning-message 2 "~A end-corods ~A~%" (send robot :name) (rotation-angle (send robot :arm :end-coords :worldrot))) - (assert (eps= (norm (send (make-coords) :difference-rotation (send robot :arm :end-coords))) 0)) + (assert (eps< (norm (send (make-coords) :difference-rotation (send robot :arm :end-coords))) 2.0)) )) (run-all-tests) (exit) - From 567db8374101e368f3caf7d013dd82959cb1669b Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Wed, 6 Jan 2021 01:13:57 +0900 Subject: [PATCH 20/41] Fix for travis test --- jsk_kinova_robot/jsk_kinova_startup/CMakeLists.txt | 9 ++++++--- .../jsk_kinova_startup/scripts/avoid_lint_error.py | 13 +++++++++++++ 2 files changed, 19 insertions(+), 3 deletions(-) create mode 100644 jsk_kinova_robot/jsk_kinova_startup/scripts/avoid_lint_error.py diff --git a/jsk_kinova_robot/jsk_kinova_startup/CMakeLists.txt b/jsk_kinova_robot/jsk_kinova_startup/CMakeLists.txt index 9013768fea..c0e3d84fbd 100644 --- a/jsk_kinova_robot/jsk_kinova_startup/CMakeLists.txt +++ b/jsk_kinova_robot/jsk_kinova_startup/CMakeLists.txt @@ -23,10 +23,13 @@ install(DIRECTORY config launch ############# if(CATKIN_ENABLE_TESTING) find_package(catkin REQUIRED COMPONENTS roslaunch roslint) + file(GLOB LAUNCH_FILES launch/*.launch) - foreach(LAUNCH_FILE ${LAUNCH_FILES}) - roslaunch_add_file_check(${LAUNCH_FILE}) - endforeach() + # TODO: Do not do roslaunch_add_file_check() + # because upstream package (kortex_driver and kinova_vision) tests do not pass + # foreach(LAUNCH_FILE ${LAUNCH_FILES}) + # roslaunch_add_file_check(${LAUNCH_FILE}) + # endforeach() set(ROSLINT_PYTHON_OPTS --max-line-length=180 --ignore=E221,E222,E241) # skip multiple spaces before/after operator roslint_python() diff --git a/jsk_kinova_robot/jsk_kinova_startup/scripts/avoid_lint_error.py b/jsk_kinova_robot/jsk_kinova_startup/scripts/avoid_lint_error.py new file mode 100644 index 0000000000..d9d50cc66f --- /dev/null +++ b/jsk_kinova_robot/jsk_kinova_startup/scripts/avoid_lint_error.py @@ -0,0 +1,13 @@ +# This file is to avoid roslint_add_test() failure in CMakeLists.txt. +# TODO: Please remove this file after other python files are created. + +# roslint_add_test() failure: +# +# +# +# +# +# From e67bece490e467517687ffaf33e437d9fcbe56e9 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Wed, 6 Jan 2021 22:09:16 +0900 Subject: [PATCH 21/41] Re-define minjerk-interpolator --- jsk_kinova_robot/kinovaeus/kinova-interface.l | 59 +++++++++++++++++++ 1 file changed, 59 insertions(+) diff --git a/jsk_kinova_robot/kinovaeus/kinova-interface.l b/jsk_kinova_robot/kinovaeus/kinova-interface.l index a3b6e0ffbd..ab2925c4c5 100644 --- a/jsk_kinova_robot/kinovaeus/kinova-interface.l +++ b/jsk_kinova_robot/kinovaeus/kinova-interface.l @@ -286,6 +286,65 @@ (cons :joint-list (send robot :arm :joint-list))))) ) +;; This method can be removed if below is merged +;; https://github.com/euslisp/jskeus/pull/595 +(defmethod minjerk-interpolator + (:interpolation + () + "Minjerk interpolator, a.k.a Hoff & Arbib + + Example code is: +(setq l (instance minjerk-interpolator :init)) +(send l :reset :position-list (list #f(1 2 3) #f(3 4 5) #f(1 2 3)) :time-list (list 0.1 0.18)) +(send l :start-interpolation) +(while (send l :interpolatingp) (send l :pass-time 0.02) (print (send l :position))) +" + (let* ((xi (nth segment position-list)) + (xf (nth (1+ segment) position-list)) + (vi (nth segment velocity-list)) + (vf (nth (1+ segment) velocity-list)) + (ai (nth segment acceleration-list)) + (af (nth (1+ segment) acceleration-list)) + ;; + (t1+t2 (- (nth segment time-list) (if (> segment 0) (nth (1- segment) time-list) 0))) ;; total time of segment + ;; A=(gx-(x+v*t+(a/2.0)*t*t))/(t*t*t) + ;; B=(gv-(v+a*t))/(t*t) + ;; C=(ga-a)/t + (A (scale (/ 1.0 (* t1+t2 t1+t2 t1+t2)) (v- xf (reduce #'v+ (list xi (scale t1+t2 vi) (scale (* t1+t2 t1+t2) (scale 0.5 ai))))))) + (B (scale (/ 1.0 (* t1+t2 t1+t2)) (v- vf (v+ vi (scale t1+t2 ai))))) + (C (scale (/ 1.0 (* t1+t2)) (v- af ai))) + ;; a0=x + ;; a1=v + ;; a2=a/2.0 + ;; a3=10*A-4*B+0.5*C + ;;; a4=(-15*A+7*B-C)/t + ;; a5=(6*A-3*B+0.5*C)/(t*t) + (a0 xi) + (a1 vi) + (a2 (scale 0.5 ai)) + (a3 (v+ (v- (scale 10 A) (scale 4 B)) (scale 0.5 C))) + (a4 (scale (/ 1.0 t1+t2) (v- (v+ (scale -15 A) (scale 7 B)) C))) + (a5 (scale (/ 1.0 t1+t2 t1+t2) (v+ (v+ (scale 6 A) (scale -3 B)) (scale 0.5 C)))) + ) + ;; x=a0+a1*t+a2*t*t+a3*t*t*t+a4*t*t*t*t+a5*t*t*t*t*t + ;; v=a1+2*a2*t+3*a3*t*t+4*a4*t*t*t+5*a5*t*t*t*t + ;; a=2*a2+6*a3*t+12*a4*t*t+20*a5*t*t*t + (setq position + (reduce #'v+ (list a0 + (scale (expt segment-time 1) a1) (scale (expt segment-time 2) a2) + (scale (expt segment-time 3) a3) (scale (expt segment-time 4) a4) + (scale (expt segment-time 5) a5))) + velocity + (reduce #'v+ (list a1 + (scale (* 2 (expt segment-time 1)) a2) (scale (* 3 (expt segment-time 2)) a3) + (scale (* 4 (expt segment-time 3)) a4) (scale (* 5 (expt segment-time 4)) a5))) + acceleration + (reduce #'v+ (list (scale 2 a2) + (scale (* 6 (expt segment-time 1)) a3) (scale (* 12 (expt segment-time 2)) a4) + (scale (* 20 (expt segment-time 3)) a5)))) + position)) +) + (defun kinova-init (&key (type nil) &rest args &alow-other-keys) (unless (boundp '*kinova*) (apply #'kinova (if type (list :type type) nil)) (send *kinova* :reset-pose)) (unless (ros::ok) (ros::roseus "kinova_eus_interface")) From 5962e936eb2bd2192f8eab0337c74a3e865af5ca Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Thu, 14 Jan 2021 19:13:16 +0900 Subject: [PATCH 22/41] Use real kinova gen3 lite robot --- jsk_kinova_robot/README.md | 7 ++++-- .../launch/kinova_bringup.launch | 23 ++++++++++++++----- jsk_kinova_robot/kinovaeus/package.xml | 2 ++ 3 files changed, 24 insertions(+), 8 deletions(-) diff --git a/jsk_kinova_robot/README.md b/jsk_kinova_robot/README.md index 0e1d677857..dd2b7c06a1 100644 --- a/jsk_kinova_robot/README.md +++ b/jsk_kinova_robot/README.md @@ -42,10 +42,13 @@ source devel/setup.bash ## Start ROS Node -Start real kinova robot +Start real kinova robot. `ip_address` can be given by both ip address and FQDN (e.g. abc.def.jp) ```bash -roslaunch jsk_kinova_startup kinova_bringup.launch +# Gen3 robot +roslaunch jsk_kinova_startup kinova_bringup.launch ip_address:=xxxx +# Gen3 lite robot +roslaunch jsk_kinova_startup kinova_bringup.launch ip_address:=xxxx arm:=gen3_lite ``` Start rviz from different terminal diff --git a/jsk_kinova_robot/jsk_kinova_startup/launch/kinova_bringup.launch b/jsk_kinova_robot/jsk_kinova_startup/launch/kinova_bringup.launch index d09f25ca4e..4e0e3b7d3a 100644 --- a/jsk_kinova_robot/jsk_kinova_startup/launch/kinova_bringup.launch +++ b/jsk_kinova_robot/jsk_kinova_startup/launch/kinova_bringup.launch @@ -1,17 +1,28 @@ - - - - + + + + + + + + + - + + + - + + + + + diff --git a/jsk_kinova_robot/kinovaeus/package.xml b/jsk_kinova_robot/kinovaeus/package.xml index 62cff09b9b..fee7e21b9a 100644 --- a/jsk_kinova_robot/kinovaeus/package.xml +++ b/jsk_kinova_robot/kinovaeus/package.xml @@ -24,6 +24,8 @@ pr2eus_moveit kortex_gazebo + + gen3_robotiq_2f_85_move_it_config gen3_robotiq_2f_140_move_it_config gen3_lite_gen3_lite_2f_move_it_config From 7b6d5cb2a75db5801d03325ee2e28ec4793e3dae Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Sat, 23 Jan 2021 18:11:54 +0900 Subject: [PATCH 23/41] move functions to robot-interface.l --- jsk_kinova_robot/kinovaeus/kinova-interface.l | 58 ------------------- 1 file changed, 58 deletions(-) diff --git a/jsk_kinova_robot/kinovaeus/kinova-interface.l b/jsk_kinova_robot/kinovaeus/kinova-interface.l index ab2925c4c5..9017afa20b 100644 --- a/jsk_kinova_robot/kinovaeus/kinova-interface.l +++ b/jsk_kinova_robot/kinovaeus/kinova-interface.l @@ -286,64 +286,6 @@ (cons :joint-list (send robot :arm :joint-list))))) ) -;; This method can be removed if below is merged -;; https://github.com/euslisp/jskeus/pull/595 -(defmethod minjerk-interpolator - (:interpolation - () - "Minjerk interpolator, a.k.a Hoff & Arbib - - Example code is: -(setq l (instance minjerk-interpolator :init)) -(send l :reset :position-list (list #f(1 2 3) #f(3 4 5) #f(1 2 3)) :time-list (list 0.1 0.18)) -(send l :start-interpolation) -(while (send l :interpolatingp) (send l :pass-time 0.02) (print (send l :position))) -" - (let* ((xi (nth segment position-list)) - (xf (nth (1+ segment) position-list)) - (vi (nth segment velocity-list)) - (vf (nth (1+ segment) velocity-list)) - (ai (nth segment acceleration-list)) - (af (nth (1+ segment) acceleration-list)) - ;; - (t1+t2 (- (nth segment time-list) (if (> segment 0) (nth (1- segment) time-list) 0))) ;; total time of segment - ;; A=(gx-(x+v*t+(a/2.0)*t*t))/(t*t*t) - ;; B=(gv-(v+a*t))/(t*t) - ;; C=(ga-a)/t - (A (scale (/ 1.0 (* t1+t2 t1+t2 t1+t2)) (v- xf (reduce #'v+ (list xi (scale t1+t2 vi) (scale (* t1+t2 t1+t2) (scale 0.5 ai))))))) - (B (scale (/ 1.0 (* t1+t2 t1+t2)) (v- vf (v+ vi (scale t1+t2 ai))))) - (C (scale (/ 1.0 (* t1+t2)) (v- af ai))) - ;; a0=x - ;; a1=v - ;; a2=a/2.0 - ;; a3=10*A-4*B+0.5*C - ;;; a4=(-15*A+7*B-C)/t - ;; a5=(6*A-3*B+0.5*C)/(t*t) - (a0 xi) - (a1 vi) - (a2 (scale 0.5 ai)) - (a3 (v+ (v- (scale 10 A) (scale 4 B)) (scale 0.5 C))) - (a4 (scale (/ 1.0 t1+t2) (v- (v+ (scale -15 A) (scale 7 B)) C))) - (a5 (scale (/ 1.0 t1+t2 t1+t2) (v+ (v+ (scale 6 A) (scale -3 B)) (scale 0.5 C)))) - ) - ;; x=a0+a1*t+a2*t*t+a3*t*t*t+a4*t*t*t*t+a5*t*t*t*t*t - ;; v=a1+2*a2*t+3*a3*t*t+4*a4*t*t*t+5*a5*t*t*t*t - ;; a=2*a2+6*a3*t+12*a4*t*t+20*a5*t*t*t - (setq position - (reduce #'v+ (list a0 - (scale (expt segment-time 1) a1) (scale (expt segment-time 2) a2) - (scale (expt segment-time 3) a3) (scale (expt segment-time 4) a4) - (scale (expt segment-time 5) a5))) - velocity - (reduce #'v+ (list a1 - (scale (* 2 (expt segment-time 1)) a2) (scale (* 3 (expt segment-time 2)) a3) - (scale (* 4 (expt segment-time 3)) a4) (scale (* 5 (expt segment-time 4)) a5))) - acceleration - (reduce #'v+ (list (scale 2 a2) - (scale (* 6 (expt segment-time 1)) a3) (scale (* 12 (expt segment-time 2)) a4) - (scale (* 20 (expt segment-time 3)) a5)))) - position)) -) (defun kinova-init (&key (type nil) &rest args &alow-other-keys) (unless (boundp '*kinova*) (apply #'kinova (if type (list :type type) nil)) (send *kinova* :reset-pose)) From 1bdf8da38cf4d40204ee8c541f9b360566e12233 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Tue, 26 Jan 2021 01:27:47 +0900 Subject: [PATCH 24/41] Use :minjerk-interpolation by default in :angle-vector --- jsk_kinova_robot/kinovaeus/kinova-interface.l | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/jsk_kinova_robot/kinovaeus/kinova-interface.l b/jsk_kinova_robot/kinovaeus/kinova-interface.l index 9017afa20b..7a2b5dd282 100644 --- a/jsk_kinova_robot/kinovaeus/kinova-interface.l +++ b/jsk_kinova_robot/kinovaeus/kinova-interface.l @@ -184,7 +184,7 @@ (setq add-new-trajectory-point t)) (incf i (send j :joint-dof))) add-new-trajectory-point)) - (:angle-vector-raw (av &optional (tm 3000) &rest args) + (:angle-vector-raw (av &optional (tm 3000) (ctype controller-type) (start-time 0) &rest args) (let* ((prev-av (send self :state :potentio-vector :wait-until-update t)) (diff-av (v- av prev-av))) (when (send self :check-continuous-joint-move-over-180 diff-av) @@ -200,10 +200,10 @@ (send minjerk :pass-time (/ tm div)) (dotimes (i div) (setq avs (append avs (list (send minjerk :pass-time (/ tm div)))))) - (send* self :angle-vector-sequence-raw avs (make-list div :initial-element (/ tm div)) args) + (send* self :angle-vector-sequence-raw avs (make-list div :initial-element (/ tm div)) ctype start-time args) (return-from :angle-vector-raw (car (last avs))))) - (send-super* :angle-vector av tm args))) - (:angle-vector-sequence-raw (&rest args) (send-super* :angle-vector-sequence args)) + (send-super* :angle-vector av tm ctype start-time :minjerk-interpolation t args))) + (:angle-vector-sequence-raw (avs &optional (tms (list 3000)) (ctype controller-type) (start-time 0.1) &rest args) (send-super* :angle-vector-sequence avs tms ctype start-time :minjerk-interpolation t args)) (:angle-vector (av &optional (tm 3000) &rest args) ;; (ctype controller-type) (start-time 0) &rest args ;; &key (clear-velocities t) &allow-other-keys) @@ -231,7 +231,8 @@ (ros::warn ":angle-vector tm is not a number, use :angle-vector av tm args")) (send* self :angle-vector-motion-plan av :ctype ctype :move-arm :rarm :total-time tm :start-offset-time (if start-offset-time start-offset-time start-time) - :clear-velocities clear-velocities args))) + :clear-velocities clear-velocities + :minjerk-interpolation t args))) (:angle-vector-sequence (avs &optional tms &rest args) ;; (ctype controller-type) (start-time 0) &rest args ;; &key (clear-velocities t) &allow-other-keys) @@ -261,7 +262,8 @@ (setq tms 3000)) (send* self :angle-vector-motion-plan avs :ctype ctype :move-arm :rarm :total-time tms :start-offset-time (if start-offset-time start-offset-time start-time) - :clear-velocities clear-velocities args))) + :clear-velocities clear-velocities + :minjerk-interpolation t args))) ) (defclass kinova-moveit-environment From 9868f91437807f261b45f51c226a63bf685c43b6 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Tue, 26 Jan 2021 15:55:43 +0900 Subject: [PATCH 25/41] Use different instance between (*ri* . robot) and *kinova* --- jsk_kinova_robot/kinovaeus/kinova-interface.l | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/jsk_kinova_robot/kinovaeus/kinova-interface.l b/jsk_kinova_robot/kinovaeus/kinova-interface.l index 7a2b5dd282..7dc9225178 100644 --- a/jsk_kinova_robot/kinovaeus/kinova-interface.l +++ b/jsk_kinova_robot/kinovaeus/kinova-interface.l @@ -15,7 +15,20 @@ (&key type &rest args) (let () (prog1 - (send-super* :init :robot (apply #'kinova (if type (list :type type) nil)) :joint-states-topic "/arm_gen3/joint_states" :groupname "kinova_interface" args) + (send-super* :init + :robot + ;; Create robot instance different from *kinova* + (case type + (:gen3_robotiq_2f_85 + (instance gen3_robotiq_2f_85-robot :init)) + (:gen3_robotiq_2f_140 + (instance gen3_robotiq_2f_140-robot :init)) + (:gen3_lite_gen3_lite_2f + (instance gen3_lite_gen3_lite_2f-robot :init)) + (t + (warning-message 1 "unknown kinova robot type ~A~%" type))) + :joint-states-topic "/arm_gen3/joint_states" + :groupname "kinova_interface" args) (send self :add-controller :arm-controller) (setq gripper-action (instance ros::simple-action-client :init From c8805eed71181d114608a94f644a910a4578a685 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Tue, 26 Jan 2021 17:02:31 +0900 Subject: [PATCH 26/41] Currently angle-vector-sequence with minjerk interpolation and MoveIt! does not work --- jsk_kinova_robot/kinovaeus/kinova-interface.l | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/jsk_kinova_robot/kinovaeus/kinova-interface.l b/jsk_kinova_robot/kinovaeus/kinova-interface.l index 7dc9225178..ff125eb810 100644 --- a/jsk_kinova_robot/kinovaeus/kinova-interface.l +++ b/jsk_kinova_robot/kinovaeus/kinova-interface.l @@ -216,7 +216,9 @@ (send* self :angle-vector-sequence-raw avs (make-list div :initial-element (/ tm div)) ctype start-time args) (return-from :angle-vector-raw (car (last avs))))) (send-super* :angle-vector av tm ctype start-time :minjerk-interpolation t args))) - (:angle-vector-sequence-raw (avs &optional (tms (list 3000)) (ctype controller-type) (start-time 0.1) &rest args) (send-super* :angle-vector-sequence avs tms ctype start-time :minjerk-interpolation t args)) + (:angle-vector-sequence-raw + (avs &optional (tms (list 3000)) (ctype controller-type) (start-time 0.1) &rest args) + (send-super* :angle-vector-sequence avs tms ctype start-time :minjerk-interpolation t args)) (:angle-vector (av &optional (tm 3000) &rest args) ;; (ctype controller-type) (start-time 0) &rest args ;; &key (clear-velocities t) &allow-other-keys) @@ -253,6 +255,10 @@ - avs : sequence of joint angle vector [rad] - tms : list of time to goal from previous angle-vector point in [msec] " + (ros::ros-warn + (format nil "~A~%~A" + ":angle-vector-sequence with minjerk-interpolation and MoveIt! sometimes returns large acceleration, which is not allowed by the actual kinova arm." + "This is because trajectory calculated by MoveIt! jumps at boundaries between angle-vectors")) (let ((ctype controller-type) (start-time 0) (start-offset-time nil) (clear-velocities t)) ;; as of 0.3.x, :angle-vector (robot-interface) :acceps tm ctype start-time as optional arguments, but in here we prefer old API (if (= (length args) 1) ;; args must be ctype From aadafe87b2aa6435c4f248c554a74989be70c87c Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Tue, 26 Jan 2021 17:05:46 +0900 Subject: [PATCH 27/41] Add url for :angle-vector-sequence --- jsk_kinova_robot/kinovaeus/kinova-interface.l | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/jsk_kinova_robot/kinovaeus/kinova-interface.l b/jsk_kinova_robot/kinovaeus/kinova-interface.l index ff125eb810..4cc81a214c 100644 --- a/jsk_kinova_robot/kinovaeus/kinova-interface.l +++ b/jsk_kinova_robot/kinovaeus/kinova-interface.l @@ -256,9 +256,10 @@ - tms : list of time to goal from previous angle-vector point in [msec] " (ros::ros-warn - (format nil "~A~%~A" + (format nil "~A~%~A~%~A" ":angle-vector-sequence with minjerk-interpolation and MoveIt! sometimes returns large acceleration, which is not allowed by the actual kinova arm." - "This is because trajectory calculated by MoveIt! jumps at boundaries between angle-vectors")) + "This is because trajectory calculated by MoveIt! jumps at boundaries between angle-vectors" + "Please see https://github.com/jsk-ros-pkg/jsk_robot/pull/1313#issuecomment-767373516")) (let ((ctype controller-type) (start-time 0) (start-offset-time nil) (clear-velocities t)) ;; as of 0.3.x, :angle-vector (robot-interface) :acceps tm ctype start-time as optional arguments, but in here we prefer old API (if (= (length args) 1) ;; args must be ctype From c40d237900bf8dc33efbdff816301206355abc18 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Fri, 29 Jan 2021 16:32:17 +0900 Subject: [PATCH 28/41] re-define robot-interface methods --- jsk_kinova_robot/kinovaeus/kinova-interface.l | 602 +++++++++++++++++- 1 file changed, 597 insertions(+), 5 deletions(-) diff --git a/jsk_kinova_robot/kinovaeus/kinova-interface.l b/jsk_kinova_robot/kinovaeus/kinova-interface.l index 4cc81a214c..bcd7cf5774 100644 --- a/jsk_kinova_robot/kinovaeus/kinova-interface.l +++ b/jsk_kinova_robot/kinovaeus/kinova-interface.l @@ -255,11 +255,6 @@ - avs : sequence of joint angle vector [rad] - tms : list of time to goal from previous angle-vector point in [msec] " - (ros::ros-warn - (format nil "~A~%~A~%~A" - ":angle-vector-sequence with minjerk-interpolation and MoveIt! sometimes returns large acceleration, which is not allowed by the actual kinova arm." - "This is because trajectory calculated by MoveIt! jumps at boundaries between angle-vectors" - "Please see https://github.com/jsk-ros-pkg/jsk_robot/pull/1313#issuecomment-767373516")) (let ((ctype controller-type) (start-time 0) (start-offset-time nil) (clear-velocities t)) ;; as of 0.3.x, :angle-vector (robot-interface) :acceps tm ctype start-time as optional arguments, but in here we prefer old API (if (= (length args) 1) ;; args must be ctype @@ -327,3 +322,600 @@ ;;(when create-viewer (objects (list *kinova*))) ) + +;; Remove defmethod robot-interface when below is merged +;; https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/456 +(defmethod robot-interface + (:angle-vector + (av &optional (tm nil) (ctype controller-type) (start-time 0) &key (scale 1) (min-time 1.0) (end-coords-interpolation nil) (end-coords-interpolation-steps 10) (minjerk-interpolation nil)) + "Send joint angle to robot, this method returns immediately, so use :wait-interpolation to block until the motion stops. +- av : joint angle vector [deg] +- tm : (time to goal in [msec]) + if designated tm is faster than fastest speed, use fastest speed + if not specified, it will use 1/scale of the fastest speed . + if :fast is specified use fastest speed calculated from max speed +- ctype : controller method name +- start-time : time to start moving +- scale : if tm is not specified, it will use 1/scale of the fastest speed +- min-time : minimum time for time to goal +- end-coords-interpolation : set t if you want to move robot in cartesian space interpolation +- end-coords-interpolation-steps : number of divisions when interpolating end-coords +- minjerk-interpolation : set t if you want to move robot with minjerk interpolation +" + (if end-coords-interpolation + (return-from :angle-vector (send self :angle-vector-sequence (list av) (list tm) ctype start-time :scale scale :min-time min-time :end-coords-interpolation t :end-coords-interpolation-steps end-coords-interpolation-steps))) + (setq ctype (or ctype controller-type)) ;; use default controller-type if ctype is nil + (unless (gethash ctype controller-table) + (warn ";; controller-type: ~A not found" ctype) + (return-from :angle-vector)) + ;;Check and decide tm + (let ((fastest-tm (* 1000 (send self :angle-vector-duration + (send self :state :potentio-vector) av scale min-time ctype)))) + (cond + ;;Fastest time Mode + ((equal tm :fast) + (setq tm fastest-tm)) + ;;Normal Number designated Mode + ((numberp tm) + (if (< tm fastest-tm) + (setq tm fastest-tm))) + ;;Safe Mode (Speed will be 5 * fastest-tm) + ((null tm) + (setq tm (* 5 fastest-tm))) + ;;Error Not Good Input + (t + (ros::ros-error ":angle-vector tm is invalid. args: ~A" tm) + (error ":angle-vector tm is invalid. args: ~A" tm))) + ) + ;; for simulation mode + (when (send self :simulation-modep) + (if av (send self :angle-vector-simulation av tm ctype))) + ;; minjerk interpolation, 0.001 [sec] inverval + (let (r trajpoints) + (if minjerk-interpolation + (progn + (setq r (pos-list-interpolation (list (send robot :angle-vector) av) + (list (/ tm 1000.0)) + 0.001)) + (setq trajpoints + (mapcar #'(lambda (pos vel tim acc) (list pos vel tim acc)) + (cadr (memq :data r)) ;; positions + (cadr (memq :velocity r)) ;; velocities + (cadr (memq :time r)) ;; duration + (cadr (memq :acceleration r)))) ;; accelerations + (nreverse trajpoints)) + (setq trajpoints + (list + (list av ;; positions + (instantiate float-vector (length av)) ;; velocities + (/ tm 1000.0) ;; duration + (instantiate float-vector (length av)))))) ;; accelerations + (send robot :angle-vector av) + (let ((cacts (gethash ctype controller-table))) + (mapcar + #'(lambda (action param) + (send self :send-ros-controller + action (cdr (assoc :joint-names param)) ;; action server and joint-names + start-time ;; start time + trajpoints)) + cacts (send self ctype)))) + av) + (:angle-vector-sequence + (avs &optional (tms (list 3000)) (ctype controller-type) (start-time 0.1) &key (scale 1) (min-time 0.0) (end-coords-interpolation nil) (end-coords-interpolation-steps 10) (minjerk-interpolation nil)) + "Send sequence of joint angle to robot, this method returns immediately, so use :wait-interpolation to block until the motion stops. +- avs: sequence of joint angles(float-vector) [deg], (list av0 av1 ... avn) +- tms: sequence of duration(float) from previous angle-vector to next goal [msec], (list tm0 tm1 ... tmn) + if tms is atom, then use (list (make-list (length avs) :initial-element tms))) for tms + if designated each tmn is faster than fastest speed, use fastest speed + if tmn is nil, then it will use 1/scale of the fastest speed . + if :fast is specified, use fastest speed calculated from max speed +- ctype : controller method name +- start-time : time to start moving +- scale : if tms is not specified, it will use 1/scale of the fastest speed +- min-time : minimum time for time to goal +- end-coords-interpolation : set t if you want to move robot in cartesian space interpolation +- end-coords-interpolation-steps : number of divisions when interpolating end-coords +- minjerk-interpolation : set t if you want to move robot with minjerk interpolation + " + (setq ctype (or ctype controller-type)) ;; use default controller-type if ctype is nil + (unless (gethash ctype controller-table) + (warn ";; controller-type: ~A not found" ctype) + (return-from :angle-vector-sequence)) + (send self :spin-once) ;; for :state :potentio-vector + (let ((st 0) (traj-points nil) + (av-prev (send self :state :potentio-vector)) av av-next + tm tm-next fastest-tm + (vel (instantiate float-vector (length (car avs))))) + (if (atom tms) (setq tms (make-list (length avs) :initial-element tms))) + (cond + ((< (length tms) (length avs)) + (nconc tms (make-list (- (length avs) (length tms)) :initial-element (car (last tms))))) + ((> (length tms) (length avs)) + (ros::ros-warn "length of tms should be the same or smaller than avs") + (setq tms (subseq tms 0 (length avs))))) + (when end-coords-interpolation ;; set end-coords interpolation + (setq min-time (/ (float min-time) end-coords-interpolation-steps)) + (let ((av-orig (send robot :angle-vector)) ;; initial av, restore at end + (c-orig (send robot :copy-worldcoords)) ;; inital coords, restore at end + (av-prev-orig av-prev) ;; prev-av + (diff-prev (instantiate float-vector (length av-prev))) diff ;; for over 360 deg turns + (limbs '(:larm :rarm :lleg :rleg)) ;; do not move :head and :torso by IK + target-limbs + (minjerk (instance minjerk-interpolator :init)) + end-coords-prev end-coords-current ec-prev ec-current + interpolated-avs interpolated-tms + tm pass-tm i p (ret t)) ;; if nil failed to interpolate + ;; set prev-av + (send robot :angle-vector av-prev) + (setq end-coords-prev (mapcar #'(lambda (limb) (send robot limb :end-coords :copy-worldcoords)) limbs)) + ;; choose moved end-coords + (setq i 0) + (dolist (av avs) + (send robot :angle-vector av) + (setq end-coords-current (mapcar #'(lambda (limb) (send robot limb :end-coords :copy-worldcoords)) limbs)) + (setq diff (v- (v- av (setq av (v+ av-prev (send self :sub-angle-vector av av-prev)))) diff-prev)) + (setq target-limbs nil) + (setq tm (elt tms i)) + (if (equal tm :fast) + (setq tm (* 1000 (send self :angle-vector-duration + av-prev av scale min-time ctype)))) + (setq pass-tm (/ tm (float end-coords-interpolation-steps))) + (dotimes (l (length limbs)) + (setq ec-prev (elt end-coords-prev l) ec-current (elt end-coords-current l)) + (when (and ec-prev ec-current + (or (> (norm (send ec-prev :difference-position ec-current)) 1) + (> (norm (send ec-prev :difference-rotation ec-current)) (deg2rad 1)))) + (push (elt limbs l) target-limbs))) + (send minjerk :reset :position-list (list #f(0) #f(1)) :time-list (list tm)) + (send robot :angle-vector av-prev) + (if target-limbs + (progn + (send minjerk :start-interpolation) + (send minjerk :pass-time pass-tm) + (while (progn (send minjerk :pass-time pass-tm) (send minjerk :interpolatingp)) + (setq p (elt (send minjerk :position) 0)) + ;; set midpoint of av as initial pose of IK + (send robot :angle-vector (midpoint p av-prev av)) + (dolist (limb target-limbs) + (setq ec-prev (elt end-coords-prev (position limb limbs)) + ec-current (elt end-coords-current (position limb limbs))) + (setq ret (and ret + (send robot limb :inverse-kinematics (midcoords p ec-prev ec-current))))) + (push (v++ diff-prev (scale p diff) (send robot :angle-vector)) interpolated-avs) + (push pass-tm interpolated-tms) + ) + (push (v++ diff-prev diff av) interpolated-avs) + (push pass-tm interpolated-tms) + ) + (progn + (push av interpolated-avs) + (push tm interpolated-tms))) + (setq end-coords-prev end-coords-current) + (setq av-prev av) + (setq diff-prev diff) + (incf i)) ;; dolist (av avs) + ;; restore states + (setq avs (nreverse interpolated-avs) tms (nreverse interpolated-tms)) + (send robot :move-to c-orig :world) + (send robot :angle-vector av-orig) + (setq av-prev av-prev-orig) + (unless ret + (warning-message 1 ":angle-vector-sequence failed to generate end-coords-interpolation motion~%") + (return-from :angle-vector-sequence nil)) + )) + (prog1 ;; angle-vector-sequence returns avs + avs + ;; minjerk interpolation, 0.001 [sec] inverval + (if minjerk-interpolation + (let (r) + (setq r (pos-list-interpolation (append (list (send robot :angle-vector)) avs) + (mapcar #'(lambda (x) (/ x 1000.0)) tms) + 0.001)) + (setq traj-points + (mapcar #'(lambda (pos vel tim acc) (list pos vel tim acc)) + (cadr (memq :data r)) ;; positions + (cadr (memq :velocity r)) ;; velocities + (cadr (memq :time r)) ;; duration + (cadr (memq :acceleration r)))) ;; accelerations + (nreverse traj-points)) + (while avs + (setq av (pop avs)) + (setq fastest-tm (* 1000 (send self :angle-vector-duration av-prev av scale min-time ctype))) + (setq tm (pop tms)) + (cond + ((equal tm :fast) + (setq tm fastest-tm)) + ((null tm) + (setq tm (* 5 fastest-tm))) + ((numberp tm) + (if (< tm fastest-tm) + (setq tm fastest-tm))) + (t (ros::ros-error ":angle-vector-sequence tm is invalid. args: ~A" tm) + (error ":angle-vector-sequence tm is invalid. args: ~A" tm))) + (if (car tms) + (progn + (setq tm-next ( car tms)) + (setq fastest-tm-next (* 1000 (send self :angle-vector-duration av (car avs) scale min-time ctype))) + (cond + ((equal tm-next :fast) + (setq tm-next fastest-tm-next)) + ((null tm) + (setq tm (* 5 fastest-tm))) + ((numberp tm-next) + (if (< tm-next fastest-tm-next) + (setq tm-next fastest-tm-next))) + (t (ros::ros-error ":angle-vector-sequence tm is invalid. args: ~A" tm) + (error ":angle-vector-sequence tm is invalid. args: ~A" tm))) + ) + (setq tm-next 1) + ) + (if (and (setq av-next (car avs)) (> tm 0) (> tm-next 0)) + (let ((v0 (send self :sub-angle-vector av av-prev)) + (v1 (send self :sub-angle-vector av-next av))) + (dotimes (i (length vel)) + (setf (elt vel i) + (if (>= (* (elt v0 i) (elt v1 i)) 0) + (* 0.5 (+ (* (/ 1000.0 tm) (elt v0 i)) + (* (/ 1000.0 tm-next) (elt v1 i)))) + 0.0))) + ) + (fill vel 0)) + ;; for simulation mode + (if (send self :simulation-modep) + (send self :angle-vector-simulation av tm ctype)) + ;; + ;; update only joints with in current controller instead of (send robot :angle-vector av) + (unless (send self :simulation-modep) + (let* ((joint-names (flatten (mapcar #'(lambda (c) (cdr (assoc :joint-names c))) (send self ctype)))) + (joint-ids (mapcar #'(lambda (joint-name) (position joint-name (send robot :joint-list) :test #'(lambda (n j) (equal n (send j :name))))) joint-names))) + (mapcar #'(lambda (name id) + (if (and (send robot :joint name) id (> (length av) id)) + (send (send robot :joint name) :joint-angle (elt av id)) + (warning-message 3 "[robot-interface.l] (angle-vector-sequence) could not find joint-name '~A' (~A) or joint-id ~A (av length ~A)~%" name (send robot :joint name) id (length av)))) + joint-names joint-ids))) +; (when (send self :simulation-modep) +; (send self :publish-joint-state) +; (if viewer (send self :draw-objects))) + + (push (list av + (copy-seq vel) ;; velocities + (/ (+ st tm) 1000.0) ;; tm + duration + (instantiate float-vector (length av))) ;; accelerations + traj-points) + (setq av-prev av) + (incf st tm))) + ;; + (let ((cacts (gethash ctype controller-table))) + (unless cacts + (warn ";; controller-type: ~A not found" ctype) + (return-from :angle-vector-sequence)) + (mapcar + #'(lambda (action param) + (send self :send-ros-controller + action (cdr (assoc :joint-names param)) ;; action server and joint-names + start-time ;; start time + traj-points)) + cacts (send self ctype))) + ))) + (:send-ros-controller + (action joint-names starttime trajpoints) + (when (send self :simulation-modep) + (return-from :send-ros-controller nil)) + (if (and warningp + (not (yes-or-no-p (format nil "~C[3~CmAre you sure to move the real robot? (~A) ~C[0m" #x1b 49 (send action :name) #x1b)))) + (return-from :send-ros-controller nil)) + (dolist (name joint-names) + (unless (send robot :joint name) + (warning-message 1 "[robot-interface.l] (send-ros-controller) could not find joint-name '~A' (~A)~%" name (send robot :joint name)) + (return-from :send-ros-controller nil))) + (let* ((goal (send action :make-goal-instance)) + (goal-points nil) + (st (if (numberp starttime) + (ros::time+ (ros::time-now) (ros::time starttime)) + starttime)) + (joints (mapcar #'(lambda (x) + (send robot (intern (string-upcase x) *keyword-package*))) + joint-names))) + (send goal :header :seq 1) + (send goal :header :stamp st) + + (cond + ((equal (class goal) control_msgs::SingleJointPositionActionGoal) + (let* ((joint (car joints)) + (id (position joint (send robot :joint-list))) + (pos (deg2rad (elt (elt (car trajpoints) 0) id))) + ) + (send goal :goal :position pos) + (send goal :goal :max_velocity 5) + (send self :spin-once) + ) + ) + (t + (send goal :goal :trajectory :joint_names joint-names) + (send goal :goal :trajectory :header :stamp st) + (dolist (trajpt trajpoints) + (let* ((all-positions (elt trajpt 0)) + (all-velocities (elt trajpt 1)) + (duration (elt trajpt 2)) + (all-accelerations (elt trajpt 3)) + (positions (instantiate float-vector (length joint-names))) + (velocities (instantiate float-vector (length joint-names))) + (accelerations (instantiate float-vector (length joint-names)))) + (dotimes (i (length joints)) + (let* ((joint (elt joints i)) + (id (position joint (send robot :joint-list))) + p v a) + (setq p (elt all-positions id) + v (elt all-velocities id) + a (elt all-accelerations id)) + (cond + ((derivedp joint rotational-joint) + (setq p (deg2rad p)) + (setq v (deg2rad v)) + (setq a (deg2rad a))) + (t + (setq p (* 0.001 p)) + (setq v (* 0.001 v)) + (setq a (* 0.001 a)))) + (setf (elt positions i) p) + (setf (elt velocities i) v) + (setf (elt accelerations i) a))) + (push (instance trajectory_msgs::JointTrajectoryPoint + :init + :positions positions + :velocities velocities + :accelerations accelerations + :time_from_start (ros::time duration)) + goal-points) + )) + (send self :spin-once) + (send goal :goal :trajectory :points goal-points) + )) + (send action :send-goal goal) + )) + (:send-trajectory (joint-trajectory-msg + &key ((:controller-type ct) controller-type) (starttime 1) (minjerk-interpolation nil) &allow-other-keys) + (unless (gethash ct controller-table) + (warn ";; controller-type: ~A not found" ct) + (return-from :send-trajectory)) + (mapcar + #'(lambda (action param) + (send self :send-trajectory-each + action (cdr (assoc :joint-names param)) ;; action server and joint-names + joint-trajectory-msg + starttime + minjerk-interpolation)) + (gethash ct controller-table) (send self ct))) + (:send-trajectory-each + (action joint-names traj &optional (starttime 0.2) (minjerk-interpolation nil)) + (let* ((jnames (send traj :joint_names)) + (ilst (mapcar #'(lambda (jn) (position jn jnames :test #'string=)) joint-names)) + points-lst) + (when (some #'identity ilst) + (setq ilst (mapcar #'(lambda (jn) + (let ((p (position jn jnames :test #'string=))) + (unless p + (setq p (send robot (intern (string-upcase jn) *keyword-package*)))) + p)) + joint-names)) + (if minjerk-interpolation + ;; minjerk interpolation, 0.001 [sec] inverval + (let ((points (send traj :points)) (tm-prev 0.0) avs tms r) + (setq avs (mapcar #'(lambda (x) (send x :positions)) points)) + (dolist (tm (mapcar #'(lambda (x) (send (send x :time_from_start) :to-sec)) points)) + (setq tms (append tms (list (- tm tm-prev)))) + (setq tm-prev tm)) + (setq r (pos-list-interpolation avs (cdr tms) 0.001)) + (setq points-lst + (mapcar #'(lambda (pos vel acc tim) + (instance trajectory_msgs::JointTrajectoryPoint :init + :positions pos + :velocities vel + :accelerations acc + :time_from_start (ros::time tim))) + (cadr (memq :data r)) + (cadr (memq :velocity r)) + (cadr (memq :acceleration r)) + (cadr (memq :time r)))) + (nreverse points-lst) + ) + (dolist (p (send traj :points)) + (let ((poss (send p :positions)) + (vels (send p :velocities)) + (effs (send p :accelerations)) + plst vlst elst) + (dolist (i ilst) + (cond + ((numberp i) + (push (elt poss i) plst) + (if (and vels (> (length vels) i)) (push (elt vels i) vlst)) + (if (and effs (> (length effs) i)) (push (elt effs i) elst))) + (t + (push (send i :ros-joint-angle) plst) + (if vels (push 0 vlst)) + (if effs (push 0 elst)) + (ros::ros-warn ";; trajectory contains lacking joint names") + ))) + (push + (instance trajectory_msgs::JointTrajectoryPoint :init + :positions (coerce (nreverse plst) float-vector) + :velocities (if vels (coerce (nreverse vlst) float-vector)) + :accelerations (if effs (coerce (nreverse elst) float-vector)) + :time_from_start (send p :time_from_start)) points-lst) + )) + ) + (let ((goal (send action :make-goal-instance)) + (st (ros::time+ (ros::time-now) (ros::time starttime)))) + (send goal :header :stamp (ros::time-now)) + (send goal :header :seq 1) + (send goal :goal :trajectory :header :stamp st) + (send goal :goal :trajectory :header :seq 1) + (send goal :goal :trajectory :joint_names joint-names) + (send goal :goal :trajectory :points (nreverse points-lst)) + (send self :spin-once) + (send action :send-goal goal)) + (apply-trajectory_point joint-names (car (last points-lst)) robot) + ) ;;; /when ilst + )) + ) + +;; Remove below when below is merged +;; https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/456 +(defun pos-list-interpolation + (pos-list ;; (list pos_1 pos_2 ... pos_N), pos_i is float-vector + time-list ;; (list dtime_1 dtime_2 ... dtime_{N-1}), dtime_i is time[s] between time at pos_{i+1} - pos_i + dt ;; dt [s] + &key (interpolator-class minjerk-interpolator) + ((:interpolator ip) (instance interpolator-class :init)) + (initial-time 0.0) (neglect-first) (vel-vector-list) (acc-vector-list)) + (let* ((data-list) (tm-list) (vel-data-list) (acc-data-list)) + (assert (= (length pos-list) (1+ (length time-list))) + (format nil "check length of pos-list(~A) and tm-list(~A)" + (length pos-list) (length time-list))) + (setq vel-vector-list + (reverse + (do ((i 0 (1+ i)) (vel-list)) + ((> i (length time-list)) vel-list) + (if (or (= i 0) (= i (length time-list))) + (push (instantiate float-vector (length (car pos-list))) vel-list) + (let* ((v0 (scale (/ 1.0 (elt time-list (1- i))) + (v- (elt pos-list i) (elt pos-list (1- i))))) + (v1 (scale (/ 1.0 (elt time-list i)) + (v- (elt pos-list (1+ i)) (elt pos-list i)))) + (v (scale 0.5 (v+ v0 v1)))) + (dotimes (i (length v)) (if (< (* (elt v0 i) (elt v1 i)) 0) (setf (elt v i) 0))) + (push v vel-list)))))) + (setq acc-vector-list + (reverse + (do ((i 0 (1+ i)) (acc-list)) + ((> i (length time-list)) acc-list) + (if (or (= i 0) (= i (length time-list))) + (push (instantiate float-vector (length (car vel-vector-list))) acc-list) + (let* ((v0 (scale (/ 1.0 (elt time-list (1- i))) + (v- (elt vel-vector-list i) (elt vel-vector-list (1- i))))) + (v1 (scale (/ 1.0 (elt time-list i)) + (v- (elt vel-vector-list (1+ i)) (elt vel-vector-list i)))) + (v (scale 0.5 (v+ v0 v1)))) + (dotimes (i (length v)) (if (< (* (elt v0 i) (elt v1 i)) 0) (setf (elt v i) 0))) + (push v acc-list)))))) + ;; (format t "=INPUT~%") + ;; (format t "time ~A~%" time-list) + ;; (format t " pos ~A~%" pos-list) + ;; (format t " vel ~A~%" vel-vector-list) + ;; (format t " acc ~A~%" acc-vector-list) + (send* ip :reset + :position-list pos-list + :time-list (let (r) (dolist (n time-list) (push (+ n (if r (car r) 0)) r)) (nreverse r)) ;; list of time[sec] from start for each control point + (append + (if vel-vector-list (list :velocity-list vel-vector-list)) + (if acc-vector-list (list :acceleration-list acc-vector-list)))) + (send ip :start-interpolation) + (while (send ip :interpolatingp) + (push (if (send ip :interpolatingp) + (+ initial-time (send ip :time)) + (+ dt (car tm-list))) tm-list) + (send ip :pass-time dt) + (push (send ip :position) data-list) + (if (find-method ip :velocity) (push (send ip :velocity) vel-data-list)) + (if (find-method ip :acceleration) (push (send ip :acceleration) acc-data-list)) + ) + ;; (format t "=OUTPUT~%") + (if (and vel-data-list acc-data-list) + (mapcar #'(lambda (tm pos vel acc) + ;; (format t "~7,5f ~7,3f ~13,1f ~13,1f~%" + ;; tm (elt pos 0) (elt vel 0) (elt acc 0)) + ) + (reverse tm-list) (reverse data-list) (reverse vel-data-list) (reverse acc-data-list))) + (append + (list :data (if neglect-first (cdr (reverse data-list)) (reverse data-list)) + :time (if neglect-first (cdr (reverse tm-list)) (reverse tm-list))) + (if (find-method ip :velocity) + (list :velocity (if neglect-first (cdr (reverse vel-data-list)) (reverse vel-data-list)))) + (if (find-method ip :acceleration) + (list :acceleration (if neglect-first (cdr (reverse acc-data-list)) (reverse acc-data-list)))) + ))) + +;; This method can be removed if below is merged +;; https://github.com/euslisp/jskeus/pull/596 +(defmethod interpolator + (:pass-time + (dt) + "process interpolation for dt[sec]" + (when interpolatingp + (setq position (send self :interpolation)) + (incf time dt) + (setq segment-time (- time (if (= segment 0) 0 (nth (1- segment) time-list)))) + (when (eps> time (nth segment time-list) (* 0.1 dt)) + ;; if time-segment is not aligned, need to fix the data (see https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/457) + (while (and (< segment segment-num) (eps> time (nth segment time-list) (* 0.1 dt))) + (setq segment-time (- time (nth segment time-list))) + (incf segment))) + (when (>= segment segment-num) + ;; adjust time and segment-time to exact position + (setq segment (1- segment-num)) + (setq time (car (last time-list))) + (setq segment-time (- time (if (= segment 0) 0 (nth (1- segment) time-list)))) + ;; re-calculate :interpolation + (setq position (send self :interpolation)) + (send self :reset)) + position)) + ) + +;; This method can be removed if below is merged +;; https://github.com/euslisp/jskeus/pull/596 +(defmethod minjerk-interpolator + (:interpolation + () + "Minjerk interpolator, a.k.a Hoff & Arbib + Example code is: +(setq l (instance minjerk-interpolator :init)) +(send l :reset :position-list (list #f(1 2 3) #f(3 4 5) #f(1 2 3)) :time-list (list 0.1 0.18)) +(send l :start-interpolation) +(while (send l :interpolatingp) (send l :pass-time 0.02) (print (send l :position))) +" + (let* ((xi (nth segment position-list)) + (xf (nth (1+ segment) position-list)) + (vi (nth segment velocity-list)) + (vf (nth (1+ segment) velocity-list)) + (ai (nth segment acceleration-list)) + (af (nth (1+ segment) acceleration-list)) + ;; + (t1+t2 (- (nth segment time-list) (if (> segment 0) (nth (1- segment) time-list) 0))) ;; total time of segment + ;; A=(gx-(x+v*t+(a/2.0)*t*t))/(t*t*t) + ;; B=(gv-(v+a*t))/(t*t) + ;; C=(ga-a)/t + (A (scale (/ 1.0 (* t1+t2 t1+t2 t1+t2)) (v- xf (reduce #'v+ (list xi (scale t1+t2 vi) (scale (* t1+t2 t1+t2) (scale 0.5 ai))))))) + (B (scale (/ 1.0 (* t1+t2 t1+t2)) (v- vf (v+ vi (scale t1+t2 ai))))) + (C (scale (/ 1.0 (* t1+t2)) (v- af ai))) + ;; a0=x + ;; a1=v + ;; a2=a/2.0 + ;; a3=10*A-4*B+0.5*C + ;;; a4=(-15*A+7*B-C)/t + ;; a5=(6*A-3*B+0.5*C)/(t*t) + (a0 xi) + (a1 vi) + (a2 (scale 0.5 ai)) + (a3 (v+ (v- (scale 10 A) (scale 4 B)) (scale 0.5 C))) + (a4 (scale (/ 1.0 t1+t2) (v- (v+ (scale -15 A) (scale 7 B)) C))) + (a5 (scale (/ 1.0 t1+t2 t1+t2) (v+ (v+ (scale 6 A) (scale -3 B)) (scale 0.5 C)))) + ) + ;; x=a0+a1*t+a2*t*t+a3*t*t*t+a4*t*t*t*t+a5*t*t*t*t*t + ;; v=a1+2*a2*t+3*a3*t*t+4*a4*t*t*t+5*a5*t*t*t*t + ;; a=2*a2+6*a3*t+12*a4*t*t+20*a5*t*t*t + (setq position + (reduce #'v+ (list a0 + (scale (expt segment-time 1) a1) (scale (expt segment-time 2) a2) + (scale (expt segment-time 3) a3) (scale (expt segment-time 4) a4) + (scale (expt segment-time 5) a5))) + velocity + (reduce #'v+ (list a1 + (scale (* 2 (expt segment-time 1)) a2) (scale (* 3 (expt segment-time 2)) a3) + (scale (* 4 (expt segment-time 3)) a4) (scale (* 5 (expt segment-time 4)) a5))) + acceleration + (reduce #'v+ (list (scale 2 a2) + (scale (* 6 (expt segment-time 1)) a3) (scale (* 12 (expt segment-time 2)) a4) + (scale (* 20 (expt segment-time 3)) a5)))) + position)) + ; + ) From d94b382e7a5083244578d48a61a3c84731cb9476 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Wed, 3 Feb 2021 01:09:39 +0900 Subject: [PATCH 29/41] Avoid minjerk's large velocity and acceleration --- jsk_kinova_robot/kinovaeus/kinova-interface.l | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/jsk_kinova_robot/kinovaeus/kinova-interface.l b/jsk_kinova_robot/kinovaeus/kinova-interface.l index bcd7cf5774..83905b4b54 100644 --- a/jsk_kinova_robot/kinovaeus/kinova-interface.l +++ b/jsk_kinova_robot/kinovaeus/kinova-interface.l @@ -377,8 +377,11 @@ (setq r (pos-list-interpolation (list (send robot :angle-vector) av) (list (/ tm 1000.0)) 0.001)) + ;; TODO: Set vel and acc (setq trajpoints - (mapcar #'(lambda (pos vel tim acc) (list pos vel tim acc)) + (mapcar #'(lambda (pos vel tim acc) + ;; (list pos vel tim acc)) ;; ideal + (list pos (make-list (length vel) :initial-element 0) tim (make-list (length acc) :initial-element 0))) ;; temporary (cadr (memq :data r)) ;; positions (cadr (memq :velocity r)) ;; velocities (cadr (memq :time r)) ;; duration @@ -511,8 +514,11 @@ (setq r (pos-list-interpolation (append (list (send robot :angle-vector)) avs) (mapcar #'(lambda (x) (/ x 1000.0)) tms) 0.001)) + ;; TODO: Set vel and acc (setq traj-points - (mapcar #'(lambda (pos vel tim acc) (list pos vel tim acc)) + (mapcar #'(lambda (pos vel tim acc) + ;; (list pos vel tim acc)) ;; ideal + (list pos (make-list (length vel) :initial-element 0) tim (make-list (length acc) :initial-element 0))) ;; temporary (cadr (memq :data r)) ;; positions (cadr (memq :velocity r)) ;; velocities (cadr (memq :time r)) ;; duration @@ -707,11 +713,14 @@ (setq tm-prev tm)) (setq r (pos-list-interpolation avs (cdr tms) 0.001)) (setq points-lst + ;; TODO: Set vel and acc (mapcar #'(lambda (pos vel acc tim) (instance trajectory_msgs::JointTrajectoryPoint :init :positions pos - :velocities vel - :accelerations acc + ;; :velocities vel ;; ideal + :velocities (make-list (length vel) :initial-element 0) ;; temporary + ;; :accelerations acc ;; ideal + :accelerations (make-list (length acc) :initial-element 0) ;; temporary :time_from_start (ros::time tim))) (cadr (memq :data r)) (cadr (memq :velocity r)) From d3e716da41f064046d9bf729ce9aaa996aa5ca7c Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Wed, 3 Feb 2021 03:42:24 +0900 Subject: [PATCH 30/41] Add kinova-interface methods to use kinova arm's original function --- jsk_kinova_robot/kinovaeus/kinova-interface.l | 47 +++++++++++++++++++ jsk_kinova_robot/kinovaeus/package.xml | 1 + 2 files changed, 48 insertions(+) diff --git a/jsk_kinova_robot/kinovaeus/kinova-interface.l b/jsk_kinova_robot/kinovaeus/kinova-interface.l index 83905b4b54..ab23a31686 100644 --- a/jsk_kinova_robot/kinovaeus/kinova-interface.l +++ b/jsk_kinova_robot/kinovaeus/kinova-interface.l @@ -1,6 +1,7 @@ (require "package://kinovaeus/kinova-utils.l") (require "package://pr2eus/robot-interface.l") (require "package://pr2eus_moveit/euslisp/robot-moveit.l") +(ros::load-ros-manifest "kortex_driver") (if (not (find-package "kinova")) (make-package "kinova")) @@ -45,6 +46,11 @@ :groupname groupname)) (setq moveit-robot (instance (eval (send (class robot) :name)) :init)) (send self :set-moveit-environment (instance kinova-moveit-environment :init :robot moveit-robot)) + (ros::advertise "/arm_gen3/in/cartesian_velocity" kortex_driver::TwistCommand) + (ros::advertise "/arm_gen3/in/joint_velocity" kortex_driver::Base_JointSpeeds) + (ros::advertise "/arm_gen3/in/clear_faults" std_msgs::Empty) + (ros::advertise "/arm_gen3/in/stop" std_msgs::Empty) + (ros::advertise "/arm_gen3/in/emergency_stop" std_msgs::Empty) self))) ;; default robot-interface methods (:default-controller () (send self :arm-controller)) @@ -279,6 +285,47 @@ :start-offset-time (if start-offset-time start-offset-time start-time) :clear-velocities clear-velocities :minjerk-interpolation t args))) + (:joint-velocity (joint-vel) + "Send cartesian joint velocity command. +joint-vel : joint velocity [rad/s]. length must equal to the number of joint. +" + (let ((joint-length (length (send robot :angle-vector))) + (bj (instance kortex_driver::Base_JointSpeeds :init)) + js) + (when (not (eq (length joint-vel) joint-length)) + (ros::ros-error (format nil "joint-vel must be the length of ~A" joint-length)) + (return-from :joint-velocity)) + (dotimes (i joint-length) + (push (instance kortex_driver::JointSpeed :init + :joint_identifier i + :value (elt joint-vel i)) + js)) + (send bj :joint_speeds (reverse js)) + (ros::publish "/arm_gen3/in/joint_velocity" bj))) + (:stop-joint-velocity () + (send self :joint-velocity + (make-list (length (send robot :angle-vector)) :initial-element 0))) + (:cartesian-velocity (linear-vel angular-vel) + "Send cartesian velocity command to end effector +- linear : linear velocity [m/s]. 3-dim float-vector. +- angular : angular velocity [rad/s]. 3-dim float-vector. +" + (let ((tc (instance kortex_driver::TwistCommand :init))) + (send tc :twist :linear_x (elt linear-vel 0)) + (send tc :twist :linear_y (elt linear-vel 1)) + (send tc :twist :linear_z (elt linear-vel 2)) + (send tc :twist :angular_x (elt angular-vel 0)) + (send tc :twist :angular_y (elt angular-vel 1)) + (send tc :twist :angular_z (elt angular-vel 2)) + (ros::publish "/arm_gen3/in/cartesian_velocity" tc))) + (:stop-cartesian-velocity () + (send self :cartesian-velocity #f(0 0 0) #f(0 0 0))) + (:clear-faults () + (ros::publish "/arm_gen3/in/clear_faults" (instance std_msgs::Empty :init))) + (:stop () + (ros::publish "/arm_gen3/in/stop" (instance std_msgs::Empty :init))) + (:emergency-stop () + (ros::publish "/arm_gen3/in/emergency_stop" (instance std_msgs::Empty :init))) ) (defclass kinova-moveit-environment diff --git a/jsk_kinova_robot/kinovaeus/package.xml b/jsk_kinova_robot/kinovaeus/package.xml index fee7e21b9a..2e7760ea11 100644 --- a/jsk_kinova_robot/kinovaeus/package.xml +++ b/jsk_kinova_robot/kinovaeus/package.xml @@ -18,6 +18,7 @@ pr2eus pr2eus_moveit kortex_description + kortex_driver roseus pr2eus From d19a6ce0eb027ba80af14d1503849e6e1afd007f Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Wed, 9 Jun 2021 16:32:15 +0900 Subject: [PATCH 31/41] add network setting information to README --- jsk_kinova_robot/README.md | 42 +++++++++++++++++++++++++++++++++++++- 1 file changed, 41 insertions(+), 1 deletion(-) diff --git a/jsk_kinova_robot/README.md b/jsk_kinova_robot/README.md index dd2b7c06a1..40f8248b69 100644 --- a/jsk_kinova_robot/README.md +++ b/jsk_kinova_robot/README.md @@ -7,6 +7,44 @@ ROS package for KINOVA Gen3/Gen3 Lite robot. - GitHub https://github.com/Kinovarobotics/ros_kortex +## How to setup the network of KINOVA robot + +This is an overview of the network setup process. +Refer to the documentation on [KINOVA resouce page](https://www.kinovarobotics.com/en/resources/technical-resources-library) if necessary. + +Kinova can be connected with a computer via **USB, Ethernet and Wi-Fi**. + +### Via USB + +Micro-B USB to type-A cable is required. + +1. Connect one end of the micro-B USB to type-A cable to the micro-B USB connector in the robot and connect the other end to the computer. +2. DHCP server on the robot base has worked correctly, and then the computer automatically is assigned an IP address. Try to connect to the robot via _Web App_ below. If not, you have to configure the computer RNDIS settings manually. See the steps below. +3. Open the network settings in your computer and set the IPv4 address:192.168.1.11 and Subnet mask:255.255.255.0. + +### Via Ethernet +USB-A to Ethernet adapter is required. + +1. Connect an Ethernet cable to the robot by using the USB type-A to Ethernet adapter. +2. On your computer, open the network settings and set the IPv4 address:192.168.2.11 and Subnet mask:255.255.255.0. +3. Try to connect to the robot via _Web App_ below. + +### Via Wi-Fi +Connecting your computer to ```133.11.xx.xxx``` is necessary. + +1. Before you begin, you need to have a wired connection between the computer and the robot. **See the chapter Ethernet or USB and complete it** and see _Open Web App_ below. +2. Open Web App and connect to the robot. +3. Go to the Wireless & Networks page under the Configurations page. +4. You can see all of the detected Wi-Fi networks. Choose the networks you use, and fill in the required information. + +### Open Web App +You can interact with the arm and perform basic tasks through an Web browser. + +1. Confirm connecting the robot with a computer. From the web browser, enter the appropriate IP address for the arm base to access the Web App. +- 192.168.1.10 if connecting via USB +- 192.168.2.10 if connecting via Ethernet +- See the robot base if connecting via Wi-Fi. You can read xx.xx.xx...jp here. +2. Fill in the credenrials in the login window and click CONNECT. ## How to setup development environment @@ -42,7 +80,9 @@ source devel/setup.bash ## Start ROS Node -Start real kinova robot. `ip_address` can be given by both ip address and FQDN (e.g. abc.def.jp) +Start real kinova robot. `ip_address` can be given by both ip address and FQDN (e.g. abc.def.jp) +Use the appropriate ip address according to the type of network you are using +The IP address is the same as when using Web App ```bash # Gen3 robot From 7023c934c0c9f68a7ca93477c77a0cd14cee5e16 Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Wed, 9 Jun 2021 16:34:23 +0900 Subject: [PATCH 32/41] *gen3* -> *ri* in README --- jsk_kinova_robot/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/jsk_kinova_robot/README.md b/jsk_kinova_robot/README.md index 40f8248b69..2d9d833fb9 100644 --- a/jsk_kinova_robot/README.md +++ b/jsk_kinova_robot/README.md @@ -17,7 +17,7 @@ Kinova can be connected with a computer via **USB, Ethernet and Wi-Fi**. ### Via USB Micro-B USB to type-A cable is required. - +TheThe 1. Connect one end of the micro-B USB to type-A cable to the micro-B USB connector in the robot and connect the other end to the computer. 2. DHCP server on the robot base has worked correctly, and then the computer automatically is assigned an IP address. Try to connect to the robot via _Web App_ below. If not, you have to configure the computer RNDIS settings manually. See the steps below. 3. Open the network settings in your computer and set the IPv4 address:192.168.1.11 and Subnet mask:255.255.255.0. @@ -132,8 +132,8 @@ You can also use move-end-rot method to turn the gripper. To open/close gripper, you can use `start-grasp` and `stop-grasp` method. ``` -(send *gen3* :start-grasp) -(send *gen3* :stop-grasp) +(send *ri* :start-grasp) +(send *ri* :stop-grasp) ``` To control real robot. you can use *ri* object. From 4728794aacaf8384f52cf253395b3d9866ac854f Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Wed, 9 Jun 2021 16:54:16 +0900 Subject: [PATCH 33/41] add Trouble shooting in README --- jsk_kinova_robot/README.md | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/jsk_kinova_robot/README.md b/jsk_kinova_robot/README.md index 2d9d833fb9..6bab1a0f25 100644 --- a/jsk_kinova_robot/README.md +++ b/jsk_kinova_robot/README.md @@ -181,5 +181,20 @@ roseus kinova-interface.l (kinova-init :type :gen3_lite_gen3_lite_2f) ``` +## Trouble shooting +- The robot doesn't work and you see the following error in the terminal where you launched the launch file. TODO + - Turn off the power + - Make a posture that does not exceed the upper of lower limit of the joint angle + - Turn it back on +```bash +----------------------------- +Error #38 +Type : TRAJECTORY_ERROR_TYPE_JOINT_POSITION_LIMIT +Identifier : TRAJECTORY_ERROR_IDENTIFIER_POSITION +Actuator : 3 +Erroneous value is -150.149 but minimum permitted is -150.115 and maximum permitted is 150.115 +Additional message is : Invalid Position - Position of actuator(2) in Trajectory Point (37) exceeds limits +----------------------------- +``` --- If you have any question, please feel free to file open at https://github.com/jsk-ros-pkg/jsk_robot/issues From 86ada1aa10c2a7c2e77ad46249ea905f742d0355 Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Wed, 9 Jun 2021 16:57:00 +0900 Subject: [PATCH 34/41] fix typo in kinova README --- jsk_kinova_robot/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/jsk_kinova_robot/README.md b/jsk_kinova_robot/README.md index 6bab1a0f25..98f52bee0d 100644 --- a/jsk_kinova_robot/README.md +++ b/jsk_kinova_robot/README.md @@ -17,7 +17,7 @@ Kinova can be connected with a computer via **USB, Ethernet and Wi-Fi**. ### Via USB Micro-B USB to type-A cable is required. -TheThe + 1. Connect one end of the micro-B USB to type-A cable to the micro-B USB connector in the robot and connect the other end to the computer. 2. DHCP server on the robot base has worked correctly, and then the computer automatically is assigned an IP address. Try to connect to the robot via _Web App_ below. If not, you have to configure the computer RNDIS settings manually. See the steps below. 3. Open the network settings in your computer and set the IPv4 address:192.168.1.11 and Subnet mask:255.255.255.0. @@ -44,7 +44,7 @@ You can interact with the arm and perform basic tasks through an Web browser. - 192.168.1.10 if connecting via USB - 192.168.2.10 if connecting via Ethernet - See the robot base if connecting via Wi-Fi. You can read xx.xx.xx...jp here. -2. Fill in the credenrials in the login window and click CONNECT. +2. Fill in the credentials in the login window and click CONNECT. ## How to setup development environment From b5036cab6e01b989f5bc3acb49cab2ba554fa212 Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Wed, 9 Jun 2021 18:36:43 +0900 Subject: [PATCH 35/41] add joint position limits to README --- jsk_kinova_robot/README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/jsk_kinova_robot/README.md b/jsk_kinova_robot/README.md index 98f52bee0d..fc1c786ac5 100644 --- a/jsk_kinova_robot/README.md +++ b/jsk_kinova_robot/README.md @@ -196,5 +196,7 @@ Erroneous value is -150.149 but minimum permitted is -150.115 and maximum permit Additional message is : Invalid Position - Position of actuator(2) in Trajectory Point (37) exceeds limits ----------------------------- ``` +- Joint limits are different from the ones in the URDF! This causes the problem above. + - See User guides(ex.[PDF:Gen3_Lite](https://artifactory.kinovaapps.com/artifactory/generic-documentation-public/Documentation/Gen3%20lite/Technical%20documentation/User%20Guide/Gen3_lite_USER_GUIDE_R03.pdf) pp.71-72) --- If you have any question, please feel free to file open at https://github.com/jsk-ros-pkg/jsk_robot/issues From b38d5ca6939d6c3d76db154e6ceffd4ea01f1a57 Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Thu, 10 Jun 2021 12:22:09 +0900 Subject: [PATCH 36/41] add links to URDF depcriptions to README --- jsk_kinova_robot/README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/jsk_kinova_robot/README.md b/jsk_kinova_robot/README.md index fc1c786ac5..805baa00ea 100644 --- a/jsk_kinova_robot/README.md +++ b/jsk_kinova_robot/README.md @@ -197,6 +197,7 @@ Additional message is : Invalid Position - Position of actuator(2) in Trajectory ----------------------------- ``` - Joint limits are different from the ones in the URDF! This causes the problem above. - - See User guides(ex.[PDF:Gen3_Lite](https://artifactory.kinovaapps.com/artifactory/generic-documentation-public/Documentation/Gen3%20lite/Technical%20documentation/User%20Guide/Gen3_lite_USER_GUIDE_R03.pdf) pp.71-72) + - See User guides(ex.[PDF:Gen3_Lite](https://artifactory.kinovaapps.com/artifactory/generic-documentation-public/Documentation/Gen3%20lite/Technical%20documentation/User%20Guide/Gen3_lite_USER_GUIDE_R03.pdf) pp.71-72) + - See URDF descriptions([gen3_7dof](https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/arms/gen3/7dof/urdf/gen3_macro.xacro), [gen3_6dof](https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro), [gen3_lite](https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/arms/gen3_lite/6dof/urdf/gen3_lite_macro.xacro) --- If you have any question, please feel free to file open at https://github.com/jsk-ros-pkg/jsk_robot/issues From 4adadb8ff4af884adcebfe4387453ca66b9d3993 Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Thu, 10 Jun 2021 12:26:31 +0900 Subject: [PATCH 37/41] fix typo in README --- jsk_kinova_robot/README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/jsk_kinova_robot/README.md b/jsk_kinova_robot/README.md index 805baa00ea..c02e7af26f 100644 --- a/jsk_kinova_robot/README.md +++ b/jsk_kinova_robot/README.md @@ -16,14 +16,14 @@ Kinova can be connected with a computer via **USB, Ethernet and Wi-Fi**. ### Via USB -Micro-B USB to type-A cable is required. +Micro-B USB to USB type-A cable is required. 1. Connect one end of the micro-B USB to type-A cable to the micro-B USB connector in the robot and connect the other end to the computer. 2. DHCP server on the robot base has worked correctly, and then the computer automatically is assigned an IP address. Try to connect to the robot via _Web App_ below. If not, you have to configure the computer RNDIS settings manually. See the steps below. 3. Open the network settings in your computer and set the IPv4 address:192.168.1.11 and Subnet mask:255.255.255.0. ### Via Ethernet -USB-A to Ethernet adapter is required. +USB type-A to Ethernet adapter is required. 1. Connect an Ethernet cable to the robot by using the USB type-A to Ethernet adapter. 2. On your computer, open the network settings and set the IPv4 address:192.168.2.11 and Subnet mask:255.255.255.0. @@ -182,9 +182,9 @@ roseus kinova-interface.l ``` ## Trouble shooting -- The robot doesn't work and you see the following error in the terminal where you launched the launch file. TODO +- The robot doesn't work without errors in roseus and you see the following error in the terminal where you launched the launch file. TODO - Turn off the power - - Make a posture that does not exceed the upper of lower limit of the joint angle + - Make a posture that does not exceed the upper or lower limit of the joint angle - Turn it back on ```bash ----------------------------- @@ -198,6 +198,6 @@ Additional message is : Invalid Position - Position of actuator(2) in Trajectory ``` - Joint limits are different from the ones in the URDF! This causes the problem above. - See User guides(ex.[PDF:Gen3_Lite](https://artifactory.kinovaapps.com/artifactory/generic-documentation-public/Documentation/Gen3%20lite/Technical%20documentation/User%20Guide/Gen3_lite_USER_GUIDE_R03.pdf) pp.71-72) - - See URDF descriptions([gen3_7dof](https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/arms/gen3/7dof/urdf/gen3_macro.xacro), [gen3_6dof](https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro), [gen3_lite](https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/arms/gen3_lite/6dof/urdf/gen3_lite_macro.xacro) + - See URDF descriptions([gen3_7dof](https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/arms/gen3/7dof/urdf/gen3_macro.xacro), [gen3_6dof](https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro), [gen3_lite](https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/arms/gen3_lite/6dof/urdf/gen3_lite_macro.xacro)) --- If you have any question, please feel free to file open at https://github.com/jsk-ros-pkg/jsk_robot/issues From 0d611dd94deb17ce85c6dba343879cab56182b4e Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Thu, 10 Jun 2021 14:38:12 +0900 Subject: [PATCH 38/41] add a new error --- jsk_kinova_robot/README.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/jsk_kinova_robot/README.md b/jsk_kinova_robot/README.md index c02e7af26f..fda94e8da6 100644 --- a/jsk_kinova_robot/README.md +++ b/jsk_kinova_robot/README.md @@ -196,8 +196,17 @@ Erroneous value is -150.149 but minimum permitted is -150.115 and maximum permit Additional message is : Invalid Position - Position of actuator(2) in Trajectory Point (37) exceeds limits ----------------------------- ``` +This error message above also can be seen in the topic `/arm_gen3/gen3_lite_joint_trajectory_controller/follow_joint_trajectory/result`. - Joint limits are different from the ones in the URDF! This causes the problem above. - See User guides(ex.[PDF:Gen3_Lite](https://artifactory.kinovaapps.com/artifactory/generic-documentation-public/Documentation/Gen3%20lite/Technical%20documentation/User%20Guide/Gen3_lite_USER_GUIDE_R03.pdf) pp.71-72) - See URDF descriptions([gen3_7dof](https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/arms/gen3/7dof/urdf/gen3_macro.xacro), [gen3_6dof](https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro), [gen3_lite](https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/arms/gen3_lite/6dof/urdf/gen3_lite_macro.xacro)) + +- Error in the initial movement in roseus (under investigation) + - When you used the robot by Web App and then tried to use it by roseus, it occured. +```bash +[ERROR] [1623300893.102460026]: Trajectory has been aborted. +[ERROR] [1623300893.102550198]: Trajectory execution failed in the arm with sub error code 69 +The starting point for the trajectory did not match the actual commanded joint angles. +``` --- If you have any question, please feel free to file open at https://github.com/jsk-ros-pkg/jsk_robot/issues From 6a66d6ff6ba1799afd2bcb55d12486242d22b244 Mon Sep 17 00:00:00 2001 From: tkmtnt7000 Date: Thu, 10 Jun 2021 22:34:13 +0900 Subject: [PATCH 39/41] add displaying error msg at kinova-interface.l --- jsk_kinova_robot/kinovaeus/kinova-interface.l | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/jsk_kinova_robot/kinovaeus/kinova-interface.l b/jsk_kinova_robot/kinovaeus/kinova-interface.l index ab23a31686..f02b2dd849 100644 --- a/jsk_kinova_robot/kinovaeus/kinova-interface.l +++ b/jsk_kinova_robot/kinovaeus/kinova-interface.l @@ -815,6 +815,43 @@ joint-vel : joint velocity [rad/s]. length must equal to the number of joint. )) ) +(in-package "ROS") +(defmethod ros::simple-action-client + (:action-result-cb + (msg) + ;; This error msg below will appear from a second time + ;; To display this error msg from the first time, use wait-interpolation + (if (< (send (send msg :result) :error_code) 0) + (progn (ros::ros-error "~A~%" (send (send msg :result) :error_string)) + (ros::ros-error "For detail, see https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_kinova_robot/README.md#trouble-shooting"))) + + (let (dummy-msg) + (ros::ros-debug "[~A] reuslt-cb ~A ~A" name-space (send msg :status :goal_id :id) (goal-status-to-string (send msg :status :status))) + (unless (send comm-state :update-result msg) + (ros::ros-warn "[~A] action-result-cb may recieved old client's goal" name-space) + (ros::ros-warn " expected goal id ~A" (and (send comm-state :action-goal) (send (send comm-state :action-goal) :goal_id :id))) + (ros::ros-warn " recieved goal id ~A" (send msg :status :goal_id :id)) + ;; https://github.com/jsk-ros-pkg/jsk_roseus/pull/519 + ;; look for exiting objects make from simple-goal-client + (do-symbols (s (find-package "ROSACTIONLIB")) + (let ((c (eval s))) + (when (and (send (send c :comm-state) :action-goal) + (string= (send msg :status :goal_id :id) + (send (send (send c :comm-state) :action-goal) :goal_id :id))) + (send c :action-result-cb msg) + (ros::ros-warn "[~A] reuslt-cb ~A -> state ~A" name-space (send msg :status :goal_id :id) (send (send c :comm-state) :state)) + ))) + (return-from :action-result-cb nil)) + (ros::ros-debug "[~A] reuslt-cb ~A -> state ~A" name-space (send msg :status :goal_id :id) (send comm-state :state)) + ;; transition to goal state + (setq dummy-msg (instance actionlib_msgs::GoalStatusArray :init)) + (send dummy-msg :status_list (list (send msg :status))) + (send dummy-msg :header (send msg :header)) + (send self :goal-status-cb dummy-msg) + )) + ) +(in-package "USER") + ;; Remove below when below is merged ;; https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/456 (defun pos-list-interpolation From 670ebb5c33c2418ec9405781cb59ebf63c2eb0cd Mon Sep 17 00:00:00 2001 From: tkmtnt7000 Date: Fri, 11 Jun 2021 16:11:27 +0900 Subject: [PATCH 40/41] display error code in action-result-cb --- jsk_kinova_robot/kinovaeus/kinova-interface.l | 50 ++++++------------- 1 file changed, 16 insertions(+), 34 deletions(-) diff --git a/jsk_kinova_robot/kinovaeus/kinova-interface.l b/jsk_kinova_robot/kinovaeus/kinova-interface.l index f02b2dd849..fa32244dc8 100644 --- a/jsk_kinova_robot/kinovaeus/kinova-interface.l +++ b/jsk_kinova_robot/kinovaeus/kinova-interface.l @@ -815,42 +815,24 @@ joint-vel : joint velocity [rad/s]. length must equal to the number of joint. )) ) -(in-package "ROS") -(defmethod ros::simple-action-client +;; Remove below when below is merged +;; https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/460 +(defun controller-result-error-to-string (i) + (elt '(SUCCESSFUL INVALID_GOAL INVALID_JOINTS OLD_HEADER_TIMESTAMP PATH_TOLERANCE_VIOLATED GOAL_TOLERANCE_VIOLATED) (* -1 i))) + +(defmethod controller-action-client (:action-result-cb (msg) - ;; This error msg below will appear from a second time - ;; To display this error msg from the first time, use wait-interpolation - (if (< (send (send msg :result) :error_code) 0) - (progn (ros::ros-error "~A~%" (send (send msg :result) :error_string)) - (ros::ros-error "For detail, see https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_kinova_robot/README.md#trouble-shooting"))) - - (let (dummy-msg) - (ros::ros-debug "[~A] reuslt-cb ~A ~A" name-space (send msg :status :goal_id :id) (goal-status-to-string (send msg :status :status))) - (unless (send comm-state :update-result msg) - (ros::ros-warn "[~A] action-result-cb may recieved old client's goal" name-space) - (ros::ros-warn " expected goal id ~A" (and (send comm-state :action-goal) (send (send comm-state :action-goal) :goal_id :id))) - (ros::ros-warn " recieved goal id ~A" (send msg :status :goal_id :id)) - ;; https://github.com/jsk-ros-pkg/jsk_roseus/pull/519 - ;; look for exiting objects make from simple-goal-client - (do-symbols (s (find-package "ROSACTIONLIB")) - (let ((c (eval s))) - (when (and (send (send c :comm-state) :action-goal) - (string= (send msg :status :goal_id :id) - (send (send (send c :comm-state) :action-goal) :goal_id :id))) - (send c :action-result-cb msg) - (ros::ros-warn "[~A] reuslt-cb ~A -> state ~A" name-space (send msg :status :goal_id :id) (send (send c :comm-state) :state)) - ))) - (return-from :action-result-cb nil)) - (ros::ros-debug "[~A] reuslt-cb ~A -> state ~A" name-space (send msg :status :goal_id :id) (send comm-state :state)) - ;; transition to goal state - (setq dummy-msg (instance actionlib_msgs::GoalStatusArray :init)) - (send dummy-msg :status_list (list (send msg :status))) - (send dummy-msg :header (send msg :header)) - (send self :goal-status-cb dummy-msg) - )) - ) -(in-package "USER") + ;; The following error messages will not appear for the first (send *ri* :angle-vector) + ;; To display it, use :wait-interpolation or (ros::spin-once groupname) + (let ((error-code (send (send msg :result) :error_code))) + (when (< error-code 0) + ;; Remove the following two lines when below is merged + ;; https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/460 + (ros::ros-error "~A~%" (send (send msg :result) :error_string)) + (ros::ros-error "error_code: ~A~%" (cons error-code (controller-result-error-to-string error-code))) + (ros::ros-error "For detail, see https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_kinova_robot/README.md#trouble-shooting"))) + (send-super :action-result-cb msg))) ;; Remove below when below is merged ;; https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/456 From 93f19b1a795fd0278e789c7bb859968bcd39d2fd Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Wed, 16 Jun 2021 18:18:20 +0900 Subject: [PATCH 41/41] Give up waiting for upstream and remove TODOs for PR merge --- jsk_kinova_robot/README.md | 3 +-- jsk_kinova_robot/kinova.rosinstall | 22 ++++--------------- jsk_kinova_robot/kinovaeus/CMakeLists.txt | 3 ++- jsk_kinova_robot/kinovaeus/kinova-interface.l | 1 + jsk_kinova_robot/kinovaeus/package.xml | 5 ++++- 5 files changed, 12 insertions(+), 22 deletions(-) diff --git a/jsk_kinova_robot/README.md b/jsk_kinova_robot/README.md index fda94e8da6..0dc0c7d6dc 100644 --- a/jsk_kinova_robot/README.md +++ b/jsk_kinova_robot/README.md @@ -68,8 +68,7 @@ Use `wstool`, `rosdep` and `catkin` to checkout and compile the source tree. 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 merge https://raw.githubusercontent.com/jsk-ros-pkg/jsk_robot/master/jsk_kinova_robot/kinova.rosinstall wstool update cd ../ source /opt/ros/melodic/setup.bash diff --git a/jsk_kinova_robot/kinova.rosinstall b/jsk_kinova_robot/kinova.rosinstall index 6243506026..fa98c00106 100644 --- a/jsk_kinova_robot/kinova.rosinstall +++ b/jsk_kinova_robot/kinova.rosinstall @@ -1,25 +1,11 @@ -# TODO -# Use jsk-ros-pkg/jsk_robot master after approved -# - git: -# local-name: jsk_robot -# uri: https://github.com/jsk-ros-pkg/jsk_robot.git -# version: master - git: local-name: jsk_robot - uri: https://github.com/708yamaguchi/jsk_robot.git - version: kinova-gen3 -# TODO -# Use Kinovarobotics/ros_kortex kinetic-devel after merging below: -# https://github.com/Kinovarobotics/ros_kortex/pull/140 -# https://github.com/Kinovarobotics/ros_kortex/pull/141 -# - git: -# local-name: ros_kortex -# uri: https://github.com/Kinovarobotics/ros_kortex -# version: kinetic-devel + uri: https://github.com/jsk-ros-pkg/jsk_robot.git + version: master - git: local-name: ros_kortex - uri: https://github.com/708yamaguchi/ros_kortex - version: jsk-pr + uri: https://github.com/Kinovarobotics/ros_kortex + version: kinetic-devel - git: local-name: ros_kortex_vision uri: https://github.com/Kinovarobotics/ros_kortex_vision diff --git a/jsk_kinova_robot/kinovaeus/CMakeLists.txt b/jsk_kinova_robot/kinovaeus/CMakeLists.txt index 617409507f..83c5b329cc 100644 --- a/jsk_kinova_robot/kinovaeus/CMakeLists.txt +++ b/jsk_kinova_robot/kinovaeus/CMakeLists.txt @@ -60,5 +60,6 @@ if(CATKIN_ENABLE_TESTING) # Test with Kinematic Simulator add_rostest(test/test-kinova-kinematic-simulator.test) # Test with Gazebo and MoveIt! - add_rostest(test/test-kinova-moveit.test) + # TODO: We can use this test after https://github.com/Kinovarobotics/ros_kortex/pull/141 is merged + # add_rostest(test/test-kinova-moveit.test) endif() diff --git a/jsk_kinova_robot/kinovaeus/kinova-interface.l b/jsk_kinova_robot/kinovaeus/kinova-interface.l index fa32244dc8..75167362c9 100644 --- a/jsk_kinova_robot/kinovaeus/kinova-interface.l +++ b/jsk_kinova_robot/kinovaeus/kinova-interface.l @@ -425,6 +425,7 @@ joint-vel : joint velocity [rad/s]. length must equal to the number of joint. (list (/ tm 1000.0)) 0.001)) ;; TODO: Set vel and acc + ;; https://github.com/jsk-ros-pkg/jsk_robot/pull/1313#issuecomment-771768492 (setq trajpoints (mapcar #'(lambda (pos vel tim acc) ;; (list pos vel tim acc)) ;; ideal diff --git a/jsk_kinova_robot/kinovaeus/package.xml b/jsk_kinova_robot/kinovaeus/package.xml index 2e7760ea11..edd55650a2 100644 --- a/jsk_kinova_robot/kinovaeus/package.xml +++ b/jsk_kinova_robot/kinovaeus/package.xml @@ -25,7 +25,10 @@ pr2eus_moveit kortex_gazebo - + + + kortex_control + gen3_robotiq_2f_85_move_it_config gen3_robotiq_2f_140_move_it_config