naoqi-interface.l: enable to pass string as joint-name

[k-okada]

try to support

(send *ri* :send-stiffness-controller (send-all (send *pepper* :rarm :joint-list) :name) #f(1.0 1.0 1.0 .1 0.4))

but Qi only support

“Head”, “LArm” and “RArm”
“LHand” and “RHand”

???
http://doc.aldebaran.com/2-4/naoqi/motion/control-stiffness.html#control-stiffness

[k-okada]

以下のプログラムを動かしたかったんだけど，関節毎にstiffnessはセットできなさそう．

(ros::roseus "roseus_pepper")

(load "package://peppereus/pepper-interface.l") ;; load modules                                                               
(setq *pepper* (pepper))          ;; creat a robot model                                                                      
;;(setq *ri* (instance pepper-interface :init :type :naoqi-controller-disabled)) ;; make connection to the real robot, use /j\
oint_angles instead of JTA                                                                                                    
(setq *ri* (instance pepper-interface :init)) ;; make connection to the real robot                                            
(objects (list *pepper*))        ;; display the robot model     

(defun test2 ()
  (send *pepper* :reset-pose)
  (send *ri* :angle-vector (send *pepper* :angle-vector) 3000)
  (send *ri* :wait-interpolation)
  (print "done")

  (send *pepper* :rarm :elbow-p :joint-angle  80)
  (send *pepper* :larm :elbow-p :joint-angle -80)
  (send *pepper* :rarm :shoulder-p :joint-angle  40)
  (send *pepper* :larm :shoulder-p :joint-angle -40)
  (send *ri* :angle-vector (send *pepper* :angle-vector) 4000)
  (send *ri* :wait-interpolation)
  (print "done")

  ;(send-all (send *pepper* :rarm :joint-list) :name)                                                                         
  )


(defun jacobian-to-stiffness (v &optional (min-s 0.1))
  (let* ((abs-v (vmax v (v- v))) (max-v (apply #'max (coerce abs-v cons))))
    (v- (fill (instantiate float-vector (length v)) 1.0) (scale (/ (- 1.0 min-s) max-v) abs-v))))

(defun test3 (q &optional (arm :rarm) (min-stiffness 0.1))
  (let (j# jq v s)
    (setq j# (send *pepper* :calc-inverse-jacobian
                   (send *pepper* :calc-jacobian-from-link-list (send *pepper* arm :links)
                         :move-target (send *pepper* arm :end-coords))))
    (setq v (transform j# q))
    (setq s (jacobian-to-stiffness v min-stiffness))
    (ros::ros-warn "send stiffness ~A to ~A" s arm)
    (send *ri* :send-stiffness-controller (send-all (send *pepper* arm :joint-list) :name) s)
    (ros::ros-warn "(send *ri* :send-stiffness-controller ~A 1.0) ;; reset stiffness" arm)
  ))

(test3 #f(1 0 0))