Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Open
wants to merge 1 commit into
base: master
Choose a base branch
from

Conversation

k-okada
Copy link
Member

@k-okada k-okada commented Jun 21, 2024

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

@github-actions github-actions bot added the naoqi label Jun 21, 2024
@k-okada
Copy link
Member Author

k-okada commented Jun 21, 2024

以下のプログラムを動かしたかったんだけど,関節毎に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))

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Projects
None yet
Development

Successfully merging this pull request may close these issues.

1 participant