From d38ee1c160110f736e7ca5acb46d63e297788739 Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Sun, 26 Apr 2026 19:03:48 +0900 Subject: [PATCH 01/10] Add jsk_generic_teleop --- jsk_generic_teleop/CMakeLists.txt | 27 ++ jsk_generic_teleop/README.md | 54 +++ .../config/jsk-nextage.repos.yaml | 17 + .../config/touch-hid.repos.yaml | 5 + .../euslisp/jsk-generic-teleop.l | 406 ++++++++++++++++++ .../launch/dual_touch_bringup.launch | 26 ++ .../launch/ps4_joy_teleop.launch | 45 ++ .../launch/spacenav_classic.launch | 6 + jsk_generic_teleop/package.xml | 23 + jsk_generic_teleop/sample/g1-omni-sample.l | 19 + .../sample/g1-spacenav-sample.l | 22 + .../sample/nextage-omni-sample.l | 19 + .../sample/nextage_spacenav_sample.l | 22 + .../sample/pr2-spacenav-sample.l | 22 + 14 files changed, 713 insertions(+) create mode 100644 jsk_generic_teleop/CMakeLists.txt create mode 100644 jsk_generic_teleop/README.md create mode 100644 jsk_generic_teleop/config/jsk-nextage.repos.yaml create mode 100644 jsk_generic_teleop/config/touch-hid.repos.yaml create mode 100644 jsk_generic_teleop/euslisp/jsk-generic-teleop.l create mode 100644 jsk_generic_teleop/launch/dual_touch_bringup.launch create mode 100644 jsk_generic_teleop/launch/ps4_joy_teleop.launch create mode 100644 jsk_generic_teleop/launch/spacenav_classic.launch create mode 100644 jsk_generic_teleop/package.xml create mode 100644 jsk_generic_teleop/sample/g1-omni-sample.l create mode 100644 jsk_generic_teleop/sample/g1-spacenav-sample.l create mode 100644 jsk_generic_teleop/sample/nextage-omni-sample.l create mode 100644 jsk_generic_teleop/sample/nextage_spacenav_sample.l create mode 100644 jsk_generic_teleop/sample/pr2-spacenav-sample.l diff --git a/jsk_generic_teleop/CMakeLists.txt b/jsk_generic_teleop/CMakeLists.txt new file mode 100644 index 000000000..0f1482c8d --- /dev/null +++ b/jsk_generic_teleop/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 2.8.3) +project(jsk_generic_teleop) + +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + roseus + sensor_msgs + std_msgs +) + +catkin_package(CATKIN_DEPENDS + geometry_msgs + roseus + sensor_msgs + std_msgs +) + +file(GLOB PYTHON_SCRIPTS scripts/*.py) +catkin_install_python( + PROGRAMS ${PYTHON_SCRIPTS} + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY config euslisp launch sample + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS +) diff --git a/jsk_generic_teleop/README.md b/jsk_generic_teleop/README.md new file mode 100644 index 000000000..76fa1bcba --- /dev/null +++ b/jsk_generic_teleop/README.md @@ -0,0 +1,54 @@ +# jsk_generic_teleop + +`jsk_generic_teleop` provides a Roseus teleoperation loop that keeps input-device handling separate from robot IK. + +The main entry point is `jsk-generic-teleop` in `euslisp/jsk-generic-teleop.l`. +It subscribes to one input device, updates target end-effector coordinates, solves IK on the Eus robot model, and sends the resulting angle-vector through the robot interface. + +## Supported input devices + +- **Touch HID/USB** `:omni`: absolute input from `omni_msgs/OmniState` +- **Space Navigator** `:spacenav`: relative input from `sensor_msgs/Joy` + + +## Usage + +Load your robot model and interface first, then call: + +```lisp +(require "package://jsk_generic_teleop/euslisp/jsk-generic-teleop.l") +(jsk-generic-teleop + :robot *robot* + :arms '(:larm :rarm) + :device-type :omni + :init-pose :reset-pose + :origin-offsets '((:larm . (:pos #f(400 100 50) + :rpy-deg #f(0 0 0))) + (:rarm . (:pos #f(400 -100 50) + :rpy-deg #f(0 0 0)))) + :ratio 0.5) +``` + +For relative input: + +```lisp +(jsk-generic-teleop + :robot *robot* + :arms '(:rarm) + :device-type :spacenav + :init-pose :reset-pose + :origin-offsets #f(0 0 0) + :ratio 1.0) +``` + +The relative `Joy` convention is axes `[x y z roll pitch yaw]`. +Buttons `0` and `1` select the first and second arm when two arms are passed. + +For Touch HID, `origin-offsets` defines where the device zero pose lands in +the robot world frame after `init-pose` is applied. A plain `#f(x y z)` still +works for position-only offsets. Use `(:pos #f(x y z) :rpy #f(roll pitch yaw))` or +`(:pos #f(x y z) :rpy-deg #f(roll pitch yaw))` when the device zero pose also +needs an orientation offset. The default absolute axis mapping follows +`nextage_tutorials/euslisp/jsk-nextage-teleop.l`; pass +`:absolute-rotation-map` when a robot needs a different device-to-robot frame +mapping. diff --git a/jsk_generic_teleop/config/jsk-nextage.repos.yaml b/jsk_generic_teleop/config/jsk-nextage.repos.yaml new file mode 100644 index 000000000..6e43e003c --- /dev/null +++ b/jsk_generic_teleop/config/jsk-nextage.repos.yaml @@ -0,0 +1,17 @@ +repositories: + rtmros/rtmros_common: + type: git + url: git@github.com:start-jsk/rtmros_common.git + version: master + rtmros/rtmros_hironx: + type: git + url: https://github.com/start-jsk/rtmros_hironx.git + version: indigo-devel + rtmros/rtmros_nextage: + type: git + url: git@github.com:Michi-Tsubaki/rtmros_nextage.git + version: add-display-launch + rtmros/rtmros_tutorials: + type: git + url: git@github.com:Michi-Tsubaki/rtmros_tutorials.git + version: add-camera-and-ft-sensor diff --git a/jsk_generic_teleop/config/touch-hid.repos.yaml b/jsk_generic_teleop/config/touch-hid.repos.yaml new file mode 100644 index 000000000..e84b6a20d --- /dev/null +++ b/jsk_generic_teleop/config/touch-hid.repos.yaml @@ -0,0 +1,5 @@ +repositories: + Geomagic_Touch_ROS_Drivers: + type: git + url: git@github.com:pazeshun/Geomagic_Touch_ROS_Drivers.git + version: dual-phantom-readme diff --git a/jsk_generic_teleop/euslisp/jsk-generic-teleop.l b/jsk_generic_teleop/euslisp/jsk-generic-teleop.l new file mode 100644 index 000000000..b3e5b043f --- /dev/null +++ b/jsk_generic_teleop/euslisp/jsk-generic-teleop.l @@ -0,0 +1,406 @@ +#!/usr/bin/env roseus + +(load "models/arrow-object.l") + +(ros::load-ros-manifest "geometry_msgs") +(ros::load-ros-manifest "sensor_msgs") +(ros::load-ros-manifest "std_msgs") +(ros::roseus-add-msgs "geometry_msgs") +(ros::roseus-add-msgs "sensor_msgs") +(ros::roseus-add-msgs "std_msgs") +(ros::roseus-add-msgs "omni_msgs") + +(defvar *jgt-robot* nil) +(defvar *jgt-arms* '(:larm :rarm)) +(defvar *jgt-device-type* :omni) +(defvar *jgt-ratio* 1.0) +(defvar *jgt-send-time* 200) +(defvar *jgt-use-torso* :unset) +(defvar *jgt-rotation-axis* t) +(defvar *jgt-stop* 10) +(defvar *jgt-revert-if-fail* t) +(defvar *jgt-check-self-collision* t) +(defvar *jgt-debug-view* nil) +(defvar *jgt-debug-target-arrow-alist* nil) +(defvar *jgt-debug-end-arrow-alist* nil) +(defvar *jgt-origin-coords-alist* nil) +(defvar *jgt-target-coords-alist* nil) +(defvar *jgt-updated-arms* nil) +(defvar *jgt-absolute-input-alist* nil) +(defvar *jgt-relative-joy* nil) +(defvar *jgt-relative-prev-joy* nil) +(defvar *jgt-active-arm* :rarm) +(defvar *jgt-relative-translation-scale* 50.0) +(defvar *jgt-relative-rotation-scale* (/ pi 16.0)) +(defvar *jgt-relative-deadzone* 0.2) +(defvar *jgt-absolute-rotation-map* + (matrix (float-vector 0 -1 0) + (float-vector 1 0 0) + (float-vector 0 0 1))) + +(defun jgt-get-param (name default) + (if (ros::has-param name) + (ros::get-param name) + default)) + +(defun jgt-arm-keyword (arm suffix) + (read-from-string (format nil "~A-~A" (string-upcase arm) (string-upcase suffix)))) + +(defun jgt-alist-get (key alist &optional default) + (let ((cell (assoc key alist))) + (if cell (cdr cell) default))) + +(defun jgt-alist-set (key value alist) + (let ((cell (assoc key alist))) + (if cell + (progn (rplacd cell value) alist) + (cons (cons key value) alist)))) + +(defun jgt-mark-updated (arm) + (if (not (member arm *jgt-updated-arms*)) + (setq *jgt-updated-arms* (cons arm *jgt-updated-arms*)))) + +(defun jgt-to-float-vector3 (value &optional (default #f(0 0 0))) + (cond + ((vectorp value) (float-vector (elt value 0) (elt value 1) (elt value 2))) + ((consp value) (float-vector (elt value 0) (elt value 1) (elt value 2))) + (t default))) + +(defun jgt-plist-get (plist key &optional default) + (let ((tail (member key plist))) + (if (and tail (cdr tail)) (cadr tail) default))) + +(defun jgt-to-rotation-matrix (value) + (cond + ((null value) nil) + ((vectorp value) (rpy-matrix (elt value 0) (elt value 1) (elt value 2))) + ((consp value) (rpy-matrix (elt value 0) (elt value 1) (elt value 2))) + (t value))) + +(defun jgt-to-origin-offset (value) + (cond + ((and (consp value) + (or (member :pos value) (member :position value) + (member :rot value) (member :rotation value) + (member :rpy value) (member :rpy-deg value))) + (let* ((has-pos (or (member :pos value) (member :position value))) + (pos (if has-pos + (jgt-to-float-vector3 + (or (jgt-plist-get value :pos nil) + (jgt-plist-get value :position nil)) + #f(0 0 0)))) + (rot (or (jgt-to-rotation-matrix (jgt-plist-get value :rot nil)) + (jgt-to-rotation-matrix (jgt-plist-get value :rotation nil)) + (jgt-to-rotation-matrix (jgt-plist-get value :rpy nil)) + (let ((rpy-deg (jgt-plist-get value :rpy-deg nil))) + (if rpy-deg + (jgt-to-rotation-matrix (scale (/ pi 180.0) + (jgt-to-float-vector3 rpy-deg)))))))) + (list :pos pos :rot rot))) + ((and value (find-method value :worldpos)) + (list :pos (send value :worldpos) :rot (send value :worldrot))) + (value + (list :pos (jgt-to-float-vector3 value) :rot nil)) + (t nil))) + +(defun jgt-normalize-offsets (arms origin-offsets) + (cond + ((null origin-offsets) + (mapcar #'(lambda (arm) (cons arm nil)) arms)) + ((and (consp origin-offsets) (consp (car origin-offsets))) + (mapcar #'(lambda (arm) + (cons arm (jgt-to-origin-offset + (jgt-alist-get arm origin-offsets nil)))) + arms)) + (t + (let ((offset (jgt-to-origin-offset origin-offsets))) + (mapcar #'(lambda (arm) (cons arm offset)) arms))))) + +(defun jgt-current-end-coords (arm) + (send (send *jgt-robot* arm :end-coords) :copy-worldcoords)) + +(defun jgt-debug-make-arrow-alist (arms suffix) + (mapcar #'(lambda (arm) + (let ((arrow (arrow :name (format nil "~A-~A" arm suffix)))) + (cons arm arrow))) + arms)) + +(defun jgt-debug-arrow-objects () + (append (mapcar #'cdr *jgt-debug-target-arrow-alist*) + (mapcar #'cdr *jgt-debug-end-arrow-alist*))) + +(defun jgt-debug-init-arrows () + (setq *jgt-debug-target-arrow-alist* + (jgt-debug-make-arrow-alist *jgt-arms* "target")) + (setq *jgt-debug-end-arrow-alist* + (jgt-debug-make-arrow-alist *jgt-arms* "end")) + (objects (append (list *jgt-robot*) (jgt-debug-arrow-objects)))) + +(defun jgt-debug-update-arrow (arrow coords) + (when (and arrow coords) + (send arrow :newcoords (send coords :copy-worldcoords)))) + +(defun jgt-debug-update-arrows () + (dolist (arm *jgt-arms*) + (jgt-debug-update-arrow + (jgt-alist-get arm *jgt-debug-target-arrow-alist*) + (jgt-alist-get arm *jgt-target-coords-alist*)) + (jgt-debug-update-arrow + (jgt-alist-get arm *jgt-debug-end-arrow-alist*) + (jgt-current-end-coords arm)))) + +(defun jgt-debug-update-arrows-if-needed () + (if *jgt-debug-view* (jgt-debug-update-arrows))) + +(defun jgt-apply-init-pose (init-pose) + (cond + ((null init-pose) nil) + ((vectorp init-pose) (send *jgt-robot* :angle-vector init-pose)) + ((symbolp init-pose) + (if (find-method *jgt-robot* init-pose) + (send *jgt-robot* init-pose) + (ros::ros-warn "init-pose method is not found: ~A" init-pose))) + ((functionp init-pose) (funcall init-pose *jgt-robot*)) + (t (ros::ros-warn "unsupported init-pose: ~A" init-pose))) + (send *ri* :angle-vector (send *jgt-robot* :angle-vector) 1000) + (send *ri* :wait-interpolation) + ) + +(defun jgt-init-origins (origin-offsets) + (let ((offset-alist (jgt-normalize-offsets *jgt-arms* origin-offsets))) + (setq *jgt-origin-coords-alist* nil) + (setq *jgt-target-coords-alist* nil) + (dolist (arm *jgt-arms*) + (let ((origin (jgt-current-end-coords arm)) + (offset (jgt-alist-get arm offset-alist nil))) + (when offset + (let ((pos (jgt-plist-get offset :pos nil)) + (rot (jgt-plist-get offset :rot nil))) + (if pos (send origin :locate pos :world)) + (if rot (send origin :transform (make-coords :rot rot) :world)))) + (setq *jgt-origin-coords-alist* + (jgt-alist-set arm origin *jgt-origin-coords-alist*)) + (setq *jgt-target-coords-alist* + (jgt-alist-set arm (send origin :copy-worldcoords) + *jgt-target-coords-alist*)))))) + +(defun jgt-omni-state-cb (arm msg) + (setq *jgt-absolute-input-alist* + (jgt-alist-set + arm + (list + (send msg :pose :position) + (send msg :pose :orientation) + (send msg :close_gripper) + (send msg :locked)) + *jgt-absolute-input-alist*))) + +(defun jgt-left-omni-state-cb (msg) + (if *jgt-arms* + (jgt-omni-state-cb (car *jgt-arms*) msg))) + +(defun jgt-right-omni-state-cb (msg) + (if (cdr *jgt-arms*) + (jgt-omni-state-cb (cadr *jgt-arms*) msg))) + +(defun jgt-relative-joy-cb (msg) + (setq *jgt-relative-prev-joy* *jgt-relative-joy*) + (setq *jgt-relative-joy* msg)) + +(defun jgt-vector-from-omni-pos (pos) + (float-vector (send pos :y) + (- 0 (send pos :x)) + (send pos :z))) + +(defun jgt-rot-from-omni-quaternion (q) + (m* *jgt-absolute-rotation-map* + (quaternion2matrix + (float-vector (send q :w) (send q :x) (send q :y) (send q :z))))) + +(defun jgt-update-absolute-targets () + (dolist (arm *jgt-arms*) + (let ((input (jgt-alist-get arm *jgt-absolute-input-alist*))) + (when input + (let ((pos (car input)) + (rot (cadr input)) + (locked (cadddr input)) + (origin (jgt-alist-get arm *jgt-origin-coords-alist*))) + (when (and pos rot origin (not locked)) + (let* ((mapped-pos (jgt-vector-from-omni-pos pos)) + (target-pos (v+ (send origin :worldpos) + (scale *jgt-ratio* mapped-pos))) + (target-rot (m* (send origin :worldrot) + (jgt-rot-from-omni-quaternion rot))) + (target (make-coords :pos target-pos :rot target-rot))) + (setq *jgt-target-coords-alist* + (jgt-alist-set arm target *jgt-target-coords-alist*)) + (jgt-mark-updated arm)))))))) + +(defun jgt-deadzone (value) + (if (> (abs value) *jgt-relative-deadzone*) value 0.0)) + +(defun jgt-button-on-p (joy index) + (and joy + (< index (length (send joy :buttons))) + (> (elt (send joy :buttons) index) 0))) + +(defun jgt-axis (joy index) + (if (and joy (< index (length (send joy :axes)))) + (jgt-deadzone (elt (send joy :axes) index)) + 0.0)) + +(defun jgt-select-relative-arm () + (when (and *jgt-relative-joy* *jgt-relative-prev-joy*) + (cond + ((and (jgt-button-on-p *jgt-relative-joy* 0) + (not (jgt-button-on-p *jgt-relative-prev-joy* 0))) + (setq *jgt-active-arm* (car *jgt-arms*)) + (ros::ros-info "active teleop arm: ~A" *jgt-active-arm*)) + ((and (jgt-button-on-p *jgt-relative-joy* 1) + (not (jgt-button-on-p *jgt-relative-prev-joy* 1)) + (cdr *jgt-arms*)) + (setq *jgt-active-arm* (cadr *jgt-arms*)) + (ros::ros-info "active teleop arm: ~A" *jgt-active-arm*))))) + +(defun jgt-update-relative-target () + (when *jgt-relative-joy* + (jgt-select-relative-arm) + (let* ((target (jgt-alist-get *jgt-active-arm* *jgt-target-coords-alist*)) + (axes (send *jgt-relative-joy* :axes))) + (when target + (let* ((x (jgt-axis *jgt-relative-joy* 0)) + (y (jgt-axis *jgt-relative-joy* 1)) + (z (jgt-axis *jgt-relative-joy* 2)) + (rr (jgt-axis *jgt-relative-joy* 3)) + (rp (jgt-axis *jgt-relative-joy* 4)) + (ry (jgt-axis *jgt-relative-joy* 5)) + (pos-diff (scale (* *jgt-ratio* *jgt-relative-translation-scale*) + (float-vector x y z))) + (rot-diff (rpy-matrix + (* *jgt-relative-rotation-scale* ry) + (* *jgt-relative-rotation-scale* rp) + (* *jgt-relative-rotation-scale* rr))) + (new-pos (v+ (send target :worldpos) pos-diff)) + (new-rot (send (send (make-coords :rot (send target :worldrot)) + :transform (make-coords :rot rot-diff) :world) + :worldrot))) + (when (some #'(lambda (v) (> (abs v) 0.0)) (coerce axes cons)) + (setq *jgt-target-coords-alist* + (jgt-alist-set + *jgt-active-arm* + (make-coords :pos new-pos :rot new-rot) + *jgt-target-coords-alist*)) + (jgt-mark-updated *jgt-active-arm*))))))) + +(defun jgt-update-targets () + (cond + ((eq *jgt-device-type* :omni) (jgt-update-absolute-targets)) + ((or (eq *jgt-device-type* :spacenav) (eq *jgt-device-type* :joy)) + (jgt-update-relative-target)) + (t (ros::ros-warn "unknown device-type: ~A" *jgt-device-type*)))) + +(defun jgt-use-torso-args () + (unless (eq *jgt-use-torso* :unset) (list :use-torso *jgt-use-torso*))) + +(defun jgt-solve-arm-ik (arm target) + (apply #'send *jgt-robot* arm :inverse-kinematics target + :rotation-axis *jgt-rotation-axis* + :stop *jgt-stop* + :revert-if-fail *jgt-revert-if-fail* + (jgt-use-torso-args))) + +(defun jgt-self-collision-ok-p () + (or (not *jgt-check-self-collision*) + (not (find-method *jgt-robot* :self-collision-check)) + (not (send *jgt-robot* :self-collision-check)))) + +(defun jgt-run-ik-step () + (setq *jgt-updated-arms* nil) + (jgt-update-targets) + (dolist (arm *jgt-arms*) + (let ((target (jgt-alist-get arm *jgt-target-coords-alist*))) + (when target + (jgt-solve-arm-ik arm target)))) + (send *ri* :angle-vector (send *jgt-robot* :angle-vector) *jgt-send-time* nil 0 :min-time 0) + (jgt-debug-update-arrows-if-needed)) + +(defun jgt-subscribe-device + (&key + (device-type *jgt-device-type*) + (left-omni-topic "/left_device/phantom/state") + (right-omni-topic "/right_device/phantom/state") + (spacenav-topic "/spacenav/joy") + (joy-topic "/joy")) + (cond + ((eq device-type :omni) + (ros::subscribe left-omni-topic omni_msgs::OmniState #'jgt-left-omni-state-cb 1) + (if (cdr *jgt-arms*) + (ros::subscribe right-omni-topic omni_msgs::OmniState #'jgt-right-omni-state-cb 1))) + ((eq device-type :spacenav) + (ros::subscribe spacenav-topic sensor_msgs::Joy #'jgt-relative-joy-cb 1)) + ((eq device-type :joy) + (ros::subscribe joy-topic sensor_msgs::Joy #'jgt-relative-joy-cb 1)) + (t (error "unknown device-type: ~A" device-type)))) + +(defun jsk-generic-teleop + (&key + robot + robot-interface + (arms '(:larm :rarm)) + (device-type :omni) + (init-pose :reset-pose) + (origin-offsets nil) + (ratio 1.0) + (send-time 200) + (rate 30) + (use-torso :unset) + (rotation-axis t) + (stop 10) + (revert-if-fail t) + (check-self-collision t) + (left-omni-topic "/left_device/phantom/state") + (right-omni-topic "/right_device/phantom/state") + (spacenav-topic "/spacenav/joy") + (joy-topic "/joy") + (relative-translation-scale 50.0) + (relative-rotation-scale (/ pi 16.0)) + (relative-deadzone 0.2) + (absolute-rotation-map *jgt-absolute-rotation-map*) + (active-arm (car arms)) + (debug-view nil)) + (setq *jgt-robot* (or robot (if (boundp '*robot*) *robot* nil))) + (if (null *jgt-robot*) (error "robot is nil. Pass :robot or bind *robot*.")) + (setq *jgt-arms* arms) + (setq *jgt-device-type* device-type) + (setq *jgt-ratio* ratio) + (setq *jgt-send-time* send-time) + (setq *jgt-use-torso* use-torso) + (setq *jgt-rotation-axis* rotation-axis) + (setq *jgt-stop* stop) + (setq *jgt-revert-if-fail* revert-if-fail) + (setq *jgt-check-self-collision* check-self-collision) + (setq *jgt-debug-view* debug-view) + (setq *jgt-relative-translation-scale* relative-translation-scale) + (setq *jgt-relative-rotation-scale* relative-rotation-scale) + (setq *jgt-relative-deadzone* relative-deadzone) + (setq *jgt-absolute-rotation-map* absolute-rotation-map) + (setq *jgt-active-arm* active-arm) + (jgt-apply-init-pose init-pose) + (jgt-init-origins origin-offsets) + (if *jgt-debug-view* (jgt-debug-init-arrows)) + (jgt-subscribe-device + :device-type device-type + :left-omni-topic left-omni-topic + :right-omni-topic right-omni-topic + :spacenav-topic spacenav-topic + :joy-topic joy-topic) + (ros::rate rate) + (ros::ros-info "jsk_generic_teleop started: device=~A arms=~A ratio=~A" + device-type arms ratio) + (while (ros::ok) + (ros::spin-once) + (jgt-run-ik-step) + (if *jgt-debug-view* (progn (send *ri* :wait-interpolation) (send *irtviewer* :draw-objects))) + (ros::sleep))) + +(provide :jsk-generic-teleop) diff --git a/jsk_generic_teleop/launch/dual_touch_bringup.launch b/jsk_generic_teleop/launch/dual_touch_bringup.launch new file mode 100644 index 000000000..ea277d138 --- /dev/null +++ b/jsk_generic_teleop/launch/dual_touch_bringup.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/jsk_generic_teleop/launch/ps4_joy_teleop.launch b/jsk_generic_teleop/launch/ps4_joy_teleop.launch new file mode 100644 index 000000000..c0207bb98 --- /dev/null +++ b/jsk_generic_teleop/launch/ps4_joy_teleop.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/jsk_generic_teleop/launch/spacenav_classic.launch b/jsk_generic_teleop/launch/spacenav_classic.launch new file mode 100644 index 000000000..757d2630e --- /dev/null +++ b/jsk_generic_teleop/launch/spacenav_classic.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/jsk_generic_teleop/package.xml b/jsk_generic_teleop/package.xml new file mode 100644 index 000000000..84ba2c363 --- /dev/null +++ b/jsk_generic_teleop/package.xml @@ -0,0 +1,23 @@ + + jsk_generic_teleop + 0.0.1 + Generic EusLisp-based arm robot teleoperation controllers. + Kei Okada + Michitoshi Tsubaki + BSD + + catkin + + geometry_msgs + roseus + sensor_msgs + std_msgs + + roseus + pr2eus + geometry_msgs + omni_msgs + sensor_msgs + std_msgs + tf + diff --git a/jsk_generic_teleop/sample/g1-omni-sample.l b/jsk_generic_teleop/sample/g1-omni-sample.l new file mode 100644 index 000000000..cd2f4e3fb --- /dev/null +++ b/jsk_generic_teleop/sample/g1-omni-sample.l @@ -0,0 +1,19 @@ +#!/usr/bin/env roseus +(ros::roseus "jsk_generic_g1_omni_teleop") +(require "package://g1eus/euslisp/g1-interface.l") +(require "package://jsk_generic_teleop/euslisp/jsk-generic-teleop.l") + +(g1-init) + +(jsk-generic-teleop + :robot *g1* + :arms '(:larm :rarm) + :device-type :omni + :init-pose :reset-pose + :revert-if-fail nil + :check-self-collision nil + :origin-offsets nil + :ratio 0.5 + :send-time 200 + :rate 30 + :debug-view t) diff --git a/jsk_generic_teleop/sample/g1-spacenav-sample.l b/jsk_generic_teleop/sample/g1-spacenav-sample.l new file mode 100644 index 000000000..392c522d0 --- /dev/null +++ b/jsk_generic_teleop/sample/g1-spacenav-sample.l @@ -0,0 +1,22 @@ +#!/usr/bin/env roseus +(ros::roseus "jsk_generic_g1_spacenav_teleop") +(require "package://g1eus/euslisp/g1-interface.l") +(require "package://jsk_generic_teleop/euslisp/jsk-generic-teleop.l") + +(g1-init) + +(jsk-generic-teleop + :robot *g1* + :arms '(:larm :rarm) + :device-type :spacenav + :init-pose nil + :origin-offsets nil + :active-arm :rarm + :ratio 0.5 + :relative-translation-scale 30.0 + :relative-rotation-scale (/ pi 24.0) + :relative-deadzone 0.2 + :spacenav-topic "/spacenav/joy" + :send-time 200 + :rate 30 + :debug-view t) diff --git a/jsk_generic_teleop/sample/nextage-omni-sample.l b/jsk_generic_teleop/sample/nextage-omni-sample.l new file mode 100644 index 000000000..80f8a359a --- /dev/null +++ b/jsk_generic_teleop/sample/nextage-omni-sample.l @@ -0,0 +1,19 @@ +#!/usr/bin/env roseus +(ros::roseus "jsk_nextage_touch_teleop") +(require "package://nextage_tutorials/euslisp/nextage-utils.l") +(require "package://jsk_generic_teleop/euslisp/jsk-generic-teleop.l") + +(nextage-init) +(jsk-generic-teleop + :robot *nextage* + :arms '(:larm :rarm) + :device-type :omni + :init-pose nil + :origin-offsets '((:larm . (:pos #f(400 100 50) + :rpy-deg #f(0 0 0))) + (:rarm . (:pos #f(400 -100 50) + :rpy-deg #f(0 0 0)))) + :ratio 0.5 + :send-time 200 + :rate 30 + :debug-view t) diff --git a/jsk_generic_teleop/sample/nextage_spacenav_sample.l b/jsk_generic_teleop/sample/nextage_spacenav_sample.l new file mode 100644 index 000000000..a1236c29b --- /dev/null +++ b/jsk_generic_teleop/sample/nextage_spacenav_sample.l @@ -0,0 +1,22 @@ +#!/usr/bin/env roseus +(ros::roseus "jsk_generic_nextage_spacenav_teleop") +(require "package://nextage_tutorials/euslisp/nextage-interface.l") +(require "package://jsk_generic_teleop/euslisp/jsk-generic-teleop.l") + +(nextage-init) + +(jsk-generic-teleop + :robot *nextage* + :arms '(:larm :rarm) + :device-type :spacenav + :init-pose t + :origin-offsets nil + :active-arm :larm + :ratio 0.5 + :relative-translation-scale 30.0 + :relative-rotation-scale (/ pi 24.0) + :relative-deadzone 0.2 + :spacenav-topic "/spacenav/joy" + :send-time 200 + :rate 30 + :debug-view t) diff --git a/jsk_generic_teleop/sample/pr2-spacenav-sample.l b/jsk_generic_teleop/sample/pr2-spacenav-sample.l new file mode 100644 index 000000000..33c815209 --- /dev/null +++ b/jsk_generic_teleop/sample/pr2-spacenav-sample.l @@ -0,0 +1,22 @@ +#!/usr/bin/env roseus +(ros::roseus "jsk_generic_pr2_spacenav_teleop") +(require "package://pr2eus/pr2-interface.l") +(require "package://jsk_generic_teleop/euslisp/jsk-generic-teleop.l") + +(pr2-init) + +(jsk-generic-teleop + :robot *pr2* + :arms '(:larm :rarm) + :device-type :spacenav + :init-pose t + :origin-offsets nil + :active-arm :larm + :ratio 0.5 + :relative-translation-scale 30.0 + :relative-rotation-scale (/ pi 24.0) + :relative-deadzone 0.2 + :spacenav-topic "/spacenav/joy" + :send-time 200 + :rate 30 + :debug-view t) From 2a1525e150f8798fefcebe26c7aaf05cd4ee5c24 Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Sun, 26 Apr 2026 19:39:41 +0900 Subject: [PATCH 02/10] [WIP] compatible with PS4/PS5 --- jsk_generic_teleop/sample/g1-ps4-sample.l | 21 +++ .../scripts/ps4_joy_dual_arm_teleop.py | 144 ++++++++++++++++++ 2 files changed, 165 insertions(+) create mode 100644 jsk_generic_teleop/sample/g1-ps4-sample.l create mode 100755 jsk_generic_teleop/scripts/ps4_joy_dual_arm_teleop.py diff --git a/jsk_generic_teleop/sample/g1-ps4-sample.l b/jsk_generic_teleop/sample/g1-ps4-sample.l new file mode 100644 index 000000000..95dc92501 --- /dev/null +++ b/jsk_generic_teleop/sample/g1-ps4-sample.l @@ -0,0 +1,21 @@ +#!/usr/bin/env roseus +(ros::roseus "jsk_generic_ps4_teleop") +(require "package://jsk_generic_teleop/euslisp/jsk-generic-teleop.l") +(require "package://g1eus/euslisp/g1-interface.l") + +(g1-init) +(jsk-generic-teleop + :robot *g1* + :arms '(:larm :rarm) + :device-type :joy + :joy-topic "/joy_rarm" + :init-pose :reset-pose + :active-arm :rarm + :ratio 0.5 + :relative-translation-scale 30.0 + :relative-rotation-scale (/ pi 24.0) + :relative-deadzone 0.15 + :send-time 200 + :rate 30 + :debug-view t + ) diff --git a/jsk_generic_teleop/scripts/ps4_joy_dual_arm_teleop.py b/jsk_generic_teleop/scripts/ps4_joy_dual_arm_teleop.py new file mode 100755 index 000000000..b193fa1c9 --- /dev/null +++ b/jsk_generic_teleop/scripts/ps4_joy_dual_arm_teleop.py @@ -0,0 +1,144 @@ +#!/usr/bin/env python3 +""" +PS4/PS5 joystick -> 6-DOF Joy (spacenav format) for jsk-generic-teleop. + +Raw axis layout (PS4 DualShock4 / PS5 DualSense via joy_node): + [0] Left stick X : left=-1, right=+1 + [1] Left stick Y : down=-1, up=+1 + [2] L2 analog : not-pressed=+1, fully-pressed=-1 + [3] Right stick X : left=-1, right=+1 + [4] Right stick Y : down=-1, up=+1 + [5] R2 analog : not-pressed=+1, fully-pressed=-1 + +Button layout: + [0] Cross (x) + [1] Circle (o) + [2] Square ([]) + [3] Triangle (^) + [4] L1 + [5] R1 + +Output topics (6-DOF Joy, same convention as /spacenav/joy): + /joy_larm (same content as /joy_rarm) + /joy_rarm + + axes[0] X = left stick Y (forward/back) + axes[1] Y = left stick X (left/right) + axes[2] Z = R2 - L2 (R2=up/+1, L2=down/-1) + axes[3] Roll = Cross - Square (Cross=+1, Square=-1) + axes[4] Pitch = right stick Y + axes[5] Yaw = right stick X + + buttons[0] = L1 (select larm / first arm in jsk-generic-teleop) + buttons[1] = R1 (select rarm / second arm in jsk-generic-teleop) + +All 6 DOF are output simultaneously (no mode switching), +matching the /spacenav/joy format consumed by jsk-generic-teleop +with :device-type :spacenav or :joy. +""" + +import rospy +from sensor_msgs.msg import Joy + +_LS_X = 0 +_LS_Y = 1 +_L2 = 2 +_RS_X = 3 +_RS_Y = 4 +_R2 = 5 + +_BTN_CROSS = 0 +_BTN_SQUARE = 3 +_BTN_L1 = 4 +_BTN_R1 = 5 + + +class PS4JoyDualArmTeleop(object): + def __init__(self): + rospy.init_node('ps4_joy_dual_arm_teleop') + + self.idx_ls_x = rospy.get_param('~axis_ls_x', _LS_X) + self.idx_ls_y = rospy.get_param('~axis_ls_y', _LS_Y) + self.idx_rs_x = rospy.get_param('~axis_rs_x', _RS_X) + self.idx_l2 = rospy.get_param('~axis_l2', _L2) + self.idx_r2 = rospy.get_param('~axis_r2', _R2) + self.idx_rs_y = rospy.get_param('~axis_rs_y', _RS_Y) + + self.btn_cross = rospy.get_param('~btn_cross', _BTN_CROSS) + self.btn_square = rospy.get_param('~btn_square', _BTN_SQUARE) + self.btn_l1 = rospy.get_param('~btn_l1', _BTN_L1) + self.btn_r1 = rospy.get_param('~btn_r1', _BTN_R1) + + self._l2_rest = None + self._r2_rest = None + + in_topic = rospy.get_param('~input_joy', '/joy') + larm_topic = rospy.get_param('~output_joy_larm', '/joy_larm') + rarm_topic = rospy.get_param('~output_joy_rarm', '/joy_rarm') + + self._pub_larm = rospy.Publisher(larm_topic, Joy, queue_size=1) + self._pub_rarm = rospy.Publisher(rarm_topic, Joy, queue_size=1) + rospy.Subscriber(in_topic, Joy, self._cb, queue_size=1) + rospy.loginfo('ps4_joy_dual_arm_teleop: %s -> %s, %s', + in_topic, larm_topic, rarm_topic) + rospy.spin() + + def _get(self, axes, idx): + if 0 <= idx < len(axes): + return float(axes[idx]) + return 0.0 + + def _btn(self, buttons, idx): + if 0 <= idx < len(buttons): + return bool(buttons[idx]) + return False + + def _trigger(self, axes, idx, rest): + span = rest - (-1.0) + if span < 1e-6: + return 0.0 + return max(0.0, min(1.0, (rest - self._get(axes, idx)) / span)) + + def _cb(self, msg): + axes = msg.axes + buttons = msg.buttons + + if self._l2_rest is None: + self._l2_rest = self._get(axes, self.idx_l2) + if self._r2_rest is None: + self._r2_rest = self._get(axes, self.idx_r2) + + ls_x = self._get(axes, self.idx_ls_x) + ls_y = self._get(axes, self.idx_ls_y) + rs_x = self._get(axes, self.idx_rs_x) + rs_y = self._get(axes, self.idx_rs_y) + + l2 = self._trigger(axes, self.idx_l2, self._l2_rest) + r2 = self._trigger(axes, self.idx_r2, self._r2_rest) + + l1_held = self._btn(buttons, self.btn_l1) + r1_held = self._btn(buttons, self.btn_r1) + square_held = self._btn(buttons, self.btn_square) + cross_held = self._btn(buttons, self.btn_cross) + + out_axes = [ + ls_y, # [0] X + ls_x, # [1] Y + r2 - l2, # [2] Z + float(cross_held) - float(square_held), # [3] Roll + rs_y, # [4] Pitch + rs_x, # [5] Yaw + ] + out_buttons = [int(l1_held), int(r1_held)] + + out = Joy() + out.header = msg.header + out.axes = out_axes + out.buttons = out_buttons + + self._pub_larm.publish(out) + self._pub_rarm.publish(out) + + +if __name__ == '__main__': + PS4JoyDualArmTeleop() From be4c35dce7c5c650e2c967bff8d4b462899e0598 Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Mon, 11 May 2026 16:49:01 +0900 Subject: [PATCH 03/10] Add pr2eus spacenav-node dependencies --- jsk_generic_teleop/package.xml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/jsk_generic_teleop/package.xml b/jsk_generic_teleop/package.xml index 84ba2c363..9574fbdd0 100644 --- a/jsk_generic_teleop/package.xml +++ b/jsk_generic_teleop/package.xml @@ -12,7 +12,9 @@ roseus sensor_msgs std_msgs - + spacenav_node + pr2eus + roseus pr2eus geometry_msgs @@ -20,4 +22,6 @@ sensor_msgs std_msgs tf + spacenav_node + pr2eus From a3d7fd695c58fbfee7a927e506352f3086ec864f Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Fri, 15 May 2026 19:02:10 +0900 Subject: [PATCH 04/10] Add hand interface to spacenav sample CC --- .../sample/g1-spacenav-sample.l | 40 +++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/jsk_generic_teleop/sample/g1-spacenav-sample.l b/jsk_generic_teleop/sample/g1-spacenav-sample.l index 392c522d0..883a5ad05 100644 --- a/jsk_generic_teleop/sample/g1-spacenav-sample.l +++ b/jsk_generic_teleop/sample/g1-spacenav-sample.l @@ -5,6 +5,46 @@ (g1-init) +(defvar *g1-spacenav-grasp-state-alist* + '((:larm . nil) (:rarm . nil))) + +(defun g1-spacenav-grasp-closed-p (arm) + (jgt-alist-get arm *g1-spacenav-grasp-state-alist* nil)) + +(defun g1-spacenav-set-grasp-closed (arm closed) + (setq *g1-spacenav-grasp-state-alist* + (jgt-alist-set arm closed *g1-spacenav-grasp-state-alist*))) + +(defun g1-spacenav-toggle-grasp (arm) + (if (g1-spacenav-grasp-closed-p arm) + (progn + (send *ri* :prepare-grasp arm) + (g1-spacenav-set-grasp-closed arm nil) + (ros::ros-info "prepare-grasp: ~A" arm)) + (progn + (send *ri* :start-grasp arm) + (g1-spacenav-set-grasp-closed arm t) + (ros::ros-info "start-grasp: ~A" arm)))) + +(defun g1-spacenav-handle-arm-button (button-index arm) + (when (and arm + (jgt-button-on-p *jgt-relative-joy* button-index) + (not (jgt-button-on-p *jgt-relative-prev-joy* button-index))) + (if (eq *jgt-active-arm* arm) + (g1-spacenav-toggle-grasp arm) + (progn + (setq *jgt-active-arm* arm) + (ros::ros-info "active teleop arm: ~A" *jgt-active-arm*))))) + +;; Override the generic spacenav button behavior for G1: +;; opposite-side button switches the teleop arm, current-side button opens/closes +;; the active hand. With :arms '(:larm :rarm), button 0 is :larm and button 1 is +;; :rarm. +(defun jgt-select-relative-arm () + (when (and *jgt-relative-joy* *jgt-relative-prev-joy*) + (g1-spacenav-handle-arm-button 0 (car *jgt-arms*)) + (g1-spacenav-handle-arm-button 1 (cadr *jgt-arms*)))) + (jsk-generic-teleop :robot *g1* :arms '(:larm :rarm) From fd43947872a80e245381bb72e9c6620b3c7c7a78 Mon Sep 17 00:00:00 2001 From: Choi Junyoung <99123abc@gmail.com> Date: Mon, 18 May 2026 16:46:04 +0900 Subject: [PATCH 05/10] Fix behavior when IK failed --- .../euslisp/jsk-generic-teleop.l | 30 +++++++++++++++---- 1 file changed, 24 insertions(+), 6 deletions(-) diff --git a/jsk_generic_teleop/euslisp/jsk-generic-teleop.l b/jsk_generic_teleop/euslisp/jsk-generic-teleop.l index b3e5b043f..95a994120 100644 --- a/jsk_generic_teleop/euslisp/jsk-generic-teleop.l +++ b/jsk_generic_teleop/euslisp/jsk-generic-teleop.l @@ -60,6 +60,14 @@ (if (not (member arm *jgt-updated-arms*)) (setq *jgt-updated-arms* (cons arm *jgt-updated-arms*)))) +(defun jgt-relative-device-p () + (or (eq *jgt-device-type* :spacenav) (eq *jgt-device-type* :joy))) + +(defun jgt-copy-coords-alist (alist) + (mapcar #'(lambda (cell) + (cons (car cell) (send (cdr cell) :copy-worldcoords))) + alist)) + (defun jgt-to-float-vector3 (value &optional (default #f(0 0 0))) (cond ((vectorp value) (float-vector (elt value 0) (elt value 1) (elt value 2))) @@ -315,12 +323,22 @@ (not (send *jgt-robot* :self-collision-check)))) (defun jgt-run-ik-step () - (setq *jgt-updated-arms* nil) - (jgt-update-targets) - (dolist (arm *jgt-arms*) - (let ((target (jgt-alist-get arm *jgt-target-coords-alist*))) - (when target - (jgt-solve-arm-ik arm target)))) + (let ((prev-target-coords-alist (jgt-copy-coords-alist *jgt-target-coords-alist*)) + (failed-arms nil)) + (setq *jgt-updated-arms* nil) + (jgt-update-targets) + (dolist (arm *jgt-arms*) + (let ((target (jgt-alist-get arm *jgt-target-coords-alist*))) + (when target + (unless (jgt-solve-arm-ik arm target) + (setq failed-arms (cons arm failed-arms)))))) + (when (jgt-relative-device-p) + (dolist (arm failed-arms) + (when (member arm *jgt-updated-arms*) + (let ((prev-target (jgt-alist-get arm prev-target-coords-alist))) + (if prev-target + (setq *jgt-target-coords-alist* + (jgt-alist-set arm prev-target *jgt-target-coords-alist*)))))))) (send *ri* :angle-vector (send *jgt-robot* :angle-vector) *jgt-send-time* nil 0 :min-time 0) (jgt-debug-update-arrows-if-needed)) From 6ae6e371d7f113e2e06c041eb434c0883fc0ab05 Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Tue, 19 May 2026 09:07:03 +0900 Subject: [PATCH 06/10] Remove dependencies of omni_msgs --- jsk_generic_teleop/euslisp/jsk-generic-teleop.l | 1 - jsk_generic_teleop/package.xml | 1 - 2 files changed, 2 deletions(-) diff --git a/jsk_generic_teleop/euslisp/jsk-generic-teleop.l b/jsk_generic_teleop/euslisp/jsk-generic-teleop.l index 95a994120..99ad35ad9 100644 --- a/jsk_generic_teleop/euslisp/jsk-generic-teleop.l +++ b/jsk_generic_teleop/euslisp/jsk-generic-teleop.l @@ -8,7 +8,6 @@ (ros::roseus-add-msgs "geometry_msgs") (ros::roseus-add-msgs "sensor_msgs") (ros::roseus-add-msgs "std_msgs") -(ros::roseus-add-msgs "omni_msgs") (defvar *jgt-robot* nil) (defvar *jgt-arms* '(:larm :rarm)) diff --git a/jsk_generic_teleop/package.xml b/jsk_generic_teleop/package.xml index 9574fbdd0..411df83b4 100644 --- a/jsk_generic_teleop/package.xml +++ b/jsk_generic_teleop/package.xml @@ -18,7 +18,6 @@ roseus pr2eus geometry_msgs - omni_msgs sensor_msgs std_msgs tf From 6c961e7507c7b6f5bfd760ca018a11eff4a7f72d Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Thu, 21 May 2026 15:25:45 +0900 Subject: [PATCH 07/10] [jsk_generic_teleop] squash teleop main euslisp node into spacenav_classic.launch --- jsk_generic_teleop/euslisp/jsk-generic-teleop.l | 0 jsk_generic_teleop/launch/spacenav_classic.launch | 1 + 2 files changed, 1 insertion(+) mode change 100644 => 100755 jsk_generic_teleop/euslisp/jsk-generic-teleop.l diff --git a/jsk_generic_teleop/euslisp/jsk-generic-teleop.l b/jsk_generic_teleop/euslisp/jsk-generic-teleop.l old mode 100644 new mode 100755 diff --git a/jsk_generic_teleop/launch/spacenav_classic.launch b/jsk_generic_teleop/launch/spacenav_classic.launch index 757d2630e..8fbc332f5 100644 --- a/jsk_generic_teleop/launch/spacenav_classic.launch +++ b/jsk_generic_teleop/launch/spacenav_classic.launch @@ -3,4 +3,5 @@ + From 647f93976a712e04b3e1d7739be0950a4318d2b8 Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Fri, 22 May 2026 14:52:20 +0900 Subject: [PATCH 08/10] Change permissions of sample scripts --- jsk_generic_teleop/euslisp/jsk-generic-teleop.l | 5 +++-- jsk_generic_teleop/sample/g1-omni-sample.l | 2 +- jsk_generic_teleop/sample/g1-ps4-sample.l | 2 +- jsk_generic_teleop/sample/g1-spacenav-sample.l | 2 +- jsk_generic_teleop/sample/nextage-omni-sample.l | 0 jsk_generic_teleop/sample/nextage_spacenav_sample.l | 0 jsk_generic_teleop/sample/pr2-spacenav-sample.l | 0 7 files changed, 6 insertions(+), 5 deletions(-) mode change 100755 => 100644 jsk_generic_teleop/euslisp/jsk-generic-teleop.l mode change 100644 => 100755 jsk_generic_teleop/sample/g1-omni-sample.l mode change 100644 => 100755 jsk_generic_teleop/sample/g1-ps4-sample.l mode change 100644 => 100755 jsk_generic_teleop/sample/g1-spacenav-sample.l mode change 100644 => 100755 jsk_generic_teleop/sample/nextage-omni-sample.l mode change 100644 => 100755 jsk_generic_teleop/sample/nextage_spacenav_sample.l mode change 100644 => 100755 jsk_generic_teleop/sample/pr2-spacenav-sample.l diff --git a/jsk_generic_teleop/euslisp/jsk-generic-teleop.l b/jsk_generic_teleop/euslisp/jsk-generic-teleop.l old mode 100755 new mode 100644 index 99ad35ad9..f9dfa9c17 --- a/jsk_generic_teleop/euslisp/jsk-generic-teleop.l +++ b/jsk_generic_teleop/euslisp/jsk-generic-teleop.l @@ -4,10 +4,11 @@ (ros::load-ros-manifest "geometry_msgs") (ros::load-ros-manifest "sensor_msgs") -(ros::load-ros-manifest "std_msgs") +(ros::load-ros-manifest "omni_msgs") (ros::roseus-add-msgs "geometry_msgs") (ros::roseus-add-msgs "sensor_msgs") -(ros::roseus-add-msgs "std_msgs") +(ros::roseus-add-msgs "omni_msgs") + (defvar *jgt-robot* nil) (defvar *jgt-arms* '(:larm :rarm)) diff --git a/jsk_generic_teleop/sample/g1-omni-sample.l b/jsk_generic_teleop/sample/g1-omni-sample.l old mode 100644 new mode 100755 index cd2f4e3fb..0ce2d918c --- a/jsk_generic_teleop/sample/g1-omni-sample.l +++ b/jsk_generic_teleop/sample/g1-omni-sample.l @@ -1,6 +1,6 @@ #!/usr/bin/env roseus (ros::roseus "jsk_generic_g1_omni_teleop") -(require "package://g1eus/euslisp/g1-interface.l") +(require "package://g1eus/g1-interface.l") (require "package://jsk_generic_teleop/euslisp/jsk-generic-teleop.l") (g1-init) diff --git a/jsk_generic_teleop/sample/g1-ps4-sample.l b/jsk_generic_teleop/sample/g1-ps4-sample.l old mode 100644 new mode 100755 index 95dc92501..50ca54ac5 --- a/jsk_generic_teleop/sample/g1-ps4-sample.l +++ b/jsk_generic_teleop/sample/g1-ps4-sample.l @@ -1,7 +1,7 @@ #!/usr/bin/env roseus (ros::roseus "jsk_generic_ps4_teleop") (require "package://jsk_generic_teleop/euslisp/jsk-generic-teleop.l") -(require "package://g1eus/euslisp/g1-interface.l") +(require "package://g1eus/g1-interface.l") (g1-init) (jsk-generic-teleop diff --git a/jsk_generic_teleop/sample/g1-spacenav-sample.l b/jsk_generic_teleop/sample/g1-spacenav-sample.l old mode 100644 new mode 100755 index 883a5ad05..c89892157 --- a/jsk_generic_teleop/sample/g1-spacenav-sample.l +++ b/jsk_generic_teleop/sample/g1-spacenav-sample.l @@ -1,6 +1,6 @@ #!/usr/bin/env roseus (ros::roseus "jsk_generic_g1_spacenav_teleop") -(require "package://g1eus/euslisp/g1-interface.l") +(require "package://g1eus/g1-interface.l") (require "package://jsk_generic_teleop/euslisp/jsk-generic-teleop.l") (g1-init) diff --git a/jsk_generic_teleop/sample/nextage-omni-sample.l b/jsk_generic_teleop/sample/nextage-omni-sample.l old mode 100644 new mode 100755 diff --git a/jsk_generic_teleop/sample/nextage_spacenav_sample.l b/jsk_generic_teleop/sample/nextage_spacenav_sample.l old mode 100644 new mode 100755 diff --git a/jsk_generic_teleop/sample/pr2-spacenav-sample.l b/jsk_generic_teleop/sample/pr2-spacenav-sample.l old mode 100644 new mode 100755 From 62d200836362428a39893fc295b030c3a4538af2 Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Fri, 22 May 2026 14:52:46 +0900 Subject: [PATCH 09/10] Add dependency information in README --- jsk_generic_teleop/README.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/jsk_generic_teleop/README.md b/jsk_generic_teleop/README.md index 76fa1bcba..050fd95a1 100644 --- a/jsk_generic_teleop/README.md +++ b/jsk_generic_teleop/README.md @@ -5,12 +5,19 @@ The main entry point is `jsk-generic-teleop` in `euslisp/jsk-generic-teleop.l`. It subscribes to one input device, updates target end-effector coordinates, solves IK on the Eus robot model, and sends the resulting angle-vector through the robot interface. + ## Supported input devices - **Touch HID/USB** `:omni`: absolute input from `omni_msgs/OmniState` - **Space Navigator** `:spacenav`: relative input from `sensor_msgs/Joy` +## Dependencies + +The following package is released, please build manually, +[omni_msgs](https://github.com/bharatm11/Geomagic_Touch_ROS_Drivers) + + ## Usage Load your robot model and interface first, then call: From e6684ca61212572316eacd30a3dcf59bc7ae4033 Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Fri, 22 May 2026 14:53:34 +0900 Subject: [PATCH 10/10] Remoeve teleop node from spacenav_classic.launch and Add g1_spacenav_teleop.launch --- jsk_generic_teleop/launch/g1_spacenav_teleop.launch | 7 +++++++ jsk_generic_teleop/launch/spacenav_classic.launch | 1 - 2 files changed, 7 insertions(+), 1 deletion(-) create mode 100644 jsk_generic_teleop/launch/g1_spacenav_teleop.launch diff --git a/jsk_generic_teleop/launch/g1_spacenav_teleop.launch b/jsk_generic_teleop/launch/g1_spacenav_teleop.launch new file mode 100644 index 000000000..a90c0c01f --- /dev/null +++ b/jsk_generic_teleop/launch/g1_spacenav_teleop.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/jsk_generic_teleop/launch/spacenav_classic.launch b/jsk_generic_teleop/launch/spacenav_classic.launch index 8fbc332f5..757d2630e 100644 --- a/jsk_generic_teleop/launch/spacenav_classic.launch +++ b/jsk_generic_teleop/launch/spacenav_classic.launch @@ -3,5 +3,4 @@ -