Set up nextage-interface.l and nextage-utils.l

Author: Michi-Tsubaki

nextage_tutorials/.gitignore

@@ -1,2 +1,5 @@
 nextage.dae
-nextage.l
+nextage.l
+launch/ft_sensor_calib.launch
+launch/manual_tf_calib.launch
+scripts/tf_to_imu.py

nextage_tutorials/euslisp/jsk-nextage-init.l

@@ -0,0 +1,18 @@
+#!/usr/bin/env roseus
+
+(require "package://nextage_tutorials/euslisp/nextage-utils.l")
+(nextage-init)
+(lhand-init)
+(rhand-init)
+
+(setq *robot* *nextage*)
+(objects (list *robot*))
+(send *robot* :reset-pose)
+(send *robot* :head :angle-vector #f(0 60))
+(send *robot* :larm :move-end-pos #f(0 0 100))
+(send *robot* :rarm :move-end-pos #f(0 0 100))
+(send *ri* :angle-vector (send *nextage* :angle-vector) 2000)
+(send *ri* :wait-interpolation)
+(send *ri* :close-holder :rarm)
+(send *ri* :open-forceps :larm)
+(send *irtviewer* :draw-objects)

…_tutorials/euslisp/jsk-nextage-teleope.l …e_tutorials/euslisp/jsk-nextage-teleop.lnextage_tutorials/euslisp/jsk-nextage-teleope.l renamed to nextage_tutorials/euslisp/jsk-nextage-teleop.l nextage_tutorials/euslisp/jsk-nextage-teleope.l renamed to nextage_tutorials/euslisp/jsk-nextage-teleop.l

@@ -1,22 +1,20 @@
 #!/usr/bin/env roseus
-;; Load packages
-(ros::load-ros-manifest "roseus")
-(ros::roseus "nextage-phantom")
+
+(ros::roseus "jsk-nextage-teleop")
 (ros::roseus-add-msgs "geometry_msgs")
 (ros::roseus-add-msgs "omni_msgs")
+
 (load "models/arrow-object.l")
-(require "package://nextage_tutorials/euslisp/nextage-interface.l")
-(require "package://dynamixel_detachable_hand/euslisp/lhand-interface.l")
-(require "package://dynamixel_detachable_hand/euslisp/rhand-interface.l")
+(require "package://nextage_tutorials/euslisp/nextage-utils.l")
 
 ;; Parameters
 (setq *use-torso* nil)
 (setq *use-hand* nil)
-(setq *base-offset-l* (float-vector 380 200 0))
-(setq *base-offset-r* (float-vector 380 -200 0))
-(setq *ratio* 3.0) ;; control ratio
-(setq *send-time* 100) ;; ms
-(ros::rate 10) ;; Hz *send-time* and rate should be balanced.
+(setq *base-offset-l* (float-vector 400 100 50))
+(setq *base-offset-r* (float-vector 400 -100 50))
+(setq *send-time* 200)
+(setq *ratio* 0.5)
+(ros::rate 5)
 
 (defun initialize-variables()
   (setq *lpos* nil)
@@ -36,34 +34,20 @@
 (defun check-work-limit()
   )
 
-(defun check-collision()
-  )
-
 (defun left-phantom-cb (msg)
   (setq *lpos* (send msg :pose :position))
   (setq *lrot* (send msg :pose :orientation))
-;;  (setq *ltf* (ros::tf-pose->coords (send msg :pose)))
-;;  (setq *lcoords* (make-coords :rot (copy-object (send *ltf* :rot))))
-;;  (send *lcoords* :rotate pi/2 :x :world) 
-;;  (send *lcoords* :locate (send *ltf* :worldpos) :world)
   (setq *lclose* (send msg :close_gripper))
   (setq *llocked* (send msg :locked))
   )
 
 (defun right-phantom-cb (msg)
   (setq *rpos* (send msg :pose :position))
   (setq *rrot* (send msg :pose :orientation))
-;;  (setq *rtf* (ros::tf-pose->coords (send msg :pose)))
-;;  (setq *rcoords* (make-coords :rot (copy-object (send *rtf* :rot))))
-;;  (send *rcoords* :rotate pi/2 :x :world) 
-;;  (send *rcoords* :locate (send *rtf* :worldpos) :world)
   (setq *rclose* (send msg :close_gripper))
   (setq *rlocked* (send msg :locked))
   )
 
-
-;; Initialize
-(ros::rate 10)
 (ros::subscribe "/left_device/phantom/state" omni_msgs::OmniState #'left-phantom-cb)
 (ros::subscribe "/right_device/phantom/state" omni_msgs::OmniState #'right-phantom-cb)
 
@@ -86,7 +70,7 @@
 
 ;; Reset robot
 (send *nextage* :reset-pose)
-(send *nextage* :head :angle-vector #f(0 10))
+(send *nextage* :head :angle-vector #f(0 60))
 (send *ri* :angle-vector (send *nextage* :angle-vector) 1000)
 (send *ri* :wait-interpolation)
 (send *irtviewer* :draw-objects)
@@ -100,7 +84,9 @@
 (do-until-key
  (ros::spin-once)
 
- (when (and *lcoords* (not *llocked*))
+ (setq ik-success t)
+ 
+ (when (and *lpos* *lrot* (not *llocked*))
    (setq *lx* (+ (* (send *lpos* :y) *ratio*) (elt *base-offset-l* 0)))
    (setq *ly* (+ (* (- 0 (send *lpos* :x)) *ratio*) (elt *base-offset-l* 1)))
    (setq *lz* (+ (* (send *lpos* :z) *ratio*) (elt *base-offset-l* 2)))
@@ -110,9 +96,8 @@
 						 (send *lrot* :z))))
    (setq lR_dst (m* R_map lR_src))
    (setq *larm-target* (make-coords :pos (float-vector *lx* *ly* *lz*) :rot lR_dst))
-   (send *la-target* :move-to *larm-target* :world)
-   (send *la-end* :move-to (send (send *nextage* :larm :end-coords) :copy-worldcoords) :world)
-   (send *nextage* :larm :inverse-kinematics *larm-target* :use-torso *use-torso* :rotation-axis t :stop 4 :revert-if-fail nil))
+   (unless (send *nextage* :larm :inverse-kinematics *larm-target* :use-torso *use-torso* :rotation-axis t :stop 10 :revert-if-fail t)
+     (setq ik-success nil)))
 
  (when (and *rpos* *rrot* (not *rlocked*))
    (setq *rx* (+ (* (send *rpos* :y) *ratio*) (elt *base-offset-r* 0)))
@@ -124,25 +109,18 @@
 						 (send *rrot* :z))))
    (setq rR_dst (m* R_map rR_src))
    (setq *rarm-target* (make-coords :pos (float-vector *rx* *ry* *rz*) :rot rR_dst))
-   (send *nextage* :rarm :inverse-kinematics *rarm-target* :use-torso *use-torso* :rotation-axis t :stop 4 :revert-if-fail nil))
+   (unless (send *nextage* :rarm :inverse-kinematics *rarm-target* :use-torso *use-torso* :rotation-axis t :stop 10 :revert-if-fail t)
+     (setq ik-success nil)))
 
- (send *ri* :angle-vector (send *nextage* :angle-vector) 100)
+ (when *lclose* (send *ri* :close-forceps :larm))
+ (unless *lclose* (send *ri* :open-forceps :larm))
+ (when *rclose* (send *ri* :close :rarm))
+ (unless *rclose* (send *ri* :open-holder :rarm))
 
-  (when (and *lclose* (not *llocked*))
-    (if *lclose*
-        (send *lhand* :close)
-      (send *lhand* :open)))
- 
-  (when (and *rclose* (not *rlocked*))
-    (if *rclose*
-        (send *rhand* :close-holder)
-      (send *rhand* :open-holder)))
+ (when (and ik-success (not (send *nextage* :self-collision-check)))
+   (send *ri* :angle-vector (send *nextage* :angle-vector) *send-time* nil 0 :min-time 0)
+   )
 
  (send *irtviewer* :draw-objects)
  (ros::sleep)
  )
-
-#|
-Memo: Using the tf conversion method descripted in https://github.com/YUKINA-3252/jsk_hironx_teleop/issues/2, we cannot control the ration of :translate, 
-So I don't use it intentionally.
-=#

nextage_tutorials/euslisp/nextage-interface.l

@@ -1,9 +1,14 @@
 (load "package://pr2eus/robot-interface.l")
 (require :nextage "package://nextage_tutorials/nextage.l")
 
+(when (ros::resolve-ros-path "package://dynamixel_detachable_hand/euslisp/lhand-interface.l")
+  (load "package://dynamixel_detachable_hand/euslisp/lhand-interface.l")
+  (load "package://dynamixel_detachable_hand/euslisp/rhand-interface.l"))
+
 (defclass nextage-interface
   :super robot-interface
   :slots ())
+
 (defmethod nextage-interface
   (:init (&rest args)
          (send-super :init :robot nextageopen-robot))
@@ -41,6 +46,50 @@
      (cons :controller-state "/head_controller/fstate")
      (cons :action-type control_msgs::FollowJointTrajectoryAction)
      (cons :joint-names (send-all (send robot :head :joint-list) :name)))))
+  (:lhand-init ()
+    (unless (boundp '*lhand*)
+      (setq *lhand* (instance lhand-interface :init)))
+    *lhand*)
+  (:rhand-init ()
+    (unless (boundp '*rhand*)
+      (setq *rhand* (instance rhand-interface :init)))
+    *rhand*)
+  (:start-grasp (&optional (arm :arms) &rest args)
+    (case arm
+      (:larm (send* (send self :lhand-init) :close args))
+      (:rarm (send* (send self :rhand-init) :close args))
+      (:arms (send* (send self :lhand-init) :close args)
+             (send* (send self :rhand-init) :close args))))
+  (:stop-grasp (&optional (arm :arms) &rest args)
+    (case arm
+      (:larm (send* (send self :lhand-init) :open args))
+      (:rarm (send* (send self :rhand-init) :open args))
+      (:arms (send* (send self :lhand-init) :open args)
+             (send* (send self :rhand-init) :open args))))
+  (:open-forceps (&optional (arm :arms) &rest args)
+    (case arm
+      (:larm (send* (send self :lhand-init) :open-forceps args))
+      (:rarm (send* (send self :rhand-init) :open-forceps args))
+      (:arms (send* (send self :lhand-init) :open-forceps args)
+             (send* (send self :rhand-init) :open-forceps args))))
+  (:close-forceps (&optional (arm :arms) &rest args)
+    (case arm
+      (:larm (send* (send self :lhand-init) :close-forceps args))
+      (:rarm (send* (send self :rhand-init) :close-forceps args))
+      (:arms (send* (send self :lhand-init) :close-forceps args)
+             (send* (send self :rhand-init) :close-forceps args))))
+  (:open-holder (&optional (arm :arms) &rest args)
+    (case arm
+      (:larm (send* (send self :lhand-init) :open-holder args))
+      (:rarm (send* (send self :rhand-init) :open-holder args))
+      (:arms (send* (send self :lhand-init) :open-holder args)
+             (send* (send self :rhand-init) :open-holder args))))
+  (:close-holder (&optional (arm :arms) &rest args)
+    (case arm
+      (:larm (send* (send self :lhand-init) :close-holder args))
+      (:rarm (send* (send self :rhand-init) :close-holder args))
+      (:arms (send* (send self :lhand-init) :close-holder args)
+             (send* (send self :rhand-init) :close-holder args))))
   )
 
 (defun nextage-init ()

nextage_tutorials/euslisp/nextage-utils.l

@@ -0,0 +1,52 @@
+(require :nextage "package://nextage_tutorials/nextage.l")
+(require "package://nextage_tutorials/euslisp/nextage-interface.l")
+
+(defmethod NextageOpen-robot
+  (:inverse-kinematics
+   (target-coords &rest args &key (avoid-collision-distance 5) &allow-other-keys)
+   (let ((r) (original-av (send self :angle-vector)))
+     (setq r (send-super* :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp nil :dump-command nil args))
+     (unless r
+       (format *error-output* "; failed for normal ik, try to start from ik-friendly position~%")
+       (let* ((move-joints (send-all (cadr (memq :link-list args)) :joint))
+              (av-after-first-ik (send self :angle-vector)))
+         (send self :angle-vector original-av)
+         (send self :reset-pose)
+         (send self :head :angle-vector #f(0 60))
+         (send self :larm :move-end-pos #f(0 0 50))
+         (send self :rarm :move-end-pos #f(0 0 50))
+         (mapcar #'(lambda (j a)
+                     (if (not (memq j move-joints))
+                         (send j :joint-angle a)))
+                 (send self :joint-list) original-av)
+         (setq r (send-super* :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp nil :dump-command nil args))
+         (unless r
+           (if (and (memq :revert-if-fail args) (null (cadr (memq :revert-if-fail args))))
+               (send self :angle-vector original-av)
+             (send self :angle-vector av-after-first-ik)))))
+     r))
+  (:l/r-reverse
+    (av)
+    (let ((av-tmp (copy-list av)))
+      (setf (elt av-tmp 0) (- (elt av-tmp 0)))
+      (setf (elt av-tmp 3) (- (elt av-tmp 3)))
+      (setf (elt av-tmp 5) (- (elt av-tmp 5)))
+      av-tmp))
+  (:self-collision-check (&rest args)
+    (let ((ret (send-super* :self-collision-check args)))
+      (when ret
+        (setq ret (remove-if #'(lambda (l)
+                                 (or (and (eq (car l) (send self :link "CHEST_JOINT0_Link"))
+                                          (eq (cdr l) (send self :link "HEAD_JOINT1_Link")))
+                                     (and (eq (cdr l) (send self :link "CHEST_JOINT0_Link"))
+                                          (eq (car l) (send self :link "HEAD_JOINT1_Link")))
+                                     (and (eq (car l) (send self :link "LARM_JOINT4_Link"))
+                                          (eq (cdr l) (send self :link "LARM_JOINT5_Link")))
+                                     (and (eq (cdr l) (send self :link "LARM_JOINT4_Link"))
+                                          (eq (car l) (send self :link "LARM_JOINT5_Link")))
+                                     (and (eq (car l) (send self :link "RARM_JOINT4_Link"))
+                                          (eq (cdr l) (send self :link "RARM_JOINT5_Link")))
+                                     (and (eq (cdr l) (send self :link "RARM_JOINT4_Link"))
+                                          (eq (car l) (send self :link "RARM_JOINT5_Link")))))
+                             ret)))
+      ret)))