Merge pull request #84 from eisoku9618/update-jaxon-red-jvrc

Update jaxon red jvrc

hrpsys_ros_bridge_jvrc/euslisp/4legs-walking.l

@@ -1,6 +1,6 @@
-(require :jaxon_red "package://hrpsys_ros_bridge_tutorials/euslisp/jaxon_red-interface.l")
+(require :jaxon_jvrc "package://hrpsys_ros_bridge_jvrc/euslisp/jaxon_jvrc-interface.l")
 
-(defun setup-robot (&key (robot 'jaxon_red) (view t))
+(defun setup-robot (&key (robot 'jaxon_jvrc) (view t))
   ;; generate robot and object models
   (unless (boundp '*robot*)
     (progn
@@ -101,17 +101,34 @@
 
 (warn "(init)~%")
 (defun init ()
-  (setup-robot :robot 'jaxon_red-init)
+  (setup-robot :robot 'jaxon_jvrc-init)
   (send *ri* :set-auto-balancer-param :transition-time 5)
   (send *ri* :start-auto-balancer)
   (send *ri* :start-grasp))
 
-(warn "(move-to-init-pose)~%")
-(defun move-to-init-pose ()
-  (dolist (avs (reset-pose-to-touch-down))
-    (send *ri* :angle-vector (cadr (memq :angle-vector avs)))
-    (send *ri* :wait-interpolation))
-  (send *ri* :stop-auto-balancer))
+(warn "(move-to-init-pose :type :biped)~%")
+(defun move-to-init-pose (&key (type :biped))
+  (case type
+    (:biped
+     (let ((avs (reset-pose-to-touch-down)))
+       (send *ri* :angle-vector (cadr (memq :angle-vector (elt avs 0))))
+       (send *ri* :wait-interpolation)
+       (send *ri* :angle-vector (cadr (memq :angle-vector (elt avs 1))))
+       (send *ri* :wait-interpolation))
+     (send *ri* :start-st)
+     (send *robot* :angle-vector (send *ri* :state :potentio-vector))
+     (send *robot* :fix-leg-to-coords (make-coords))
+     (send *robot* :rarm :angle-vector #f(+80.0 0.0 -90.0 -90.0 -120.0 -90.0 0.0 -80.0))
+     (send *robot* :larm :angle-vector #f(-80.0 0.0 +90.0 +90.0 -120.0 +90.0 0.0 -80.0))
+     (send *robot* :move-centroid-on-foot :both '(:rleg :lleg))
+     (send *ri* :angle-vector (send *robot* :angle-vector) 500)
+     (send *ri* :wait-interpolation))
+    (:quadruped
+     (dolist (avs (reset-pose-to-touch-down))
+       (send *ri* :angle-vector (cadr (memq :angle-vector avs)))
+       (send *ri* :wait-interpolation))
+     (send *ri* :stop-auto-balancer))
+    ))
 
 (defun go-pos-quad-params->footstep-list (&key (x 0) (y 0) (th 0) (type :crawl))
   (let* ((tmp-fsl (send *robot* :go-pos-params->footstep-list x y th

hrpsys_ros_bridge_jvrc/euslisp/jaxon_jvrc-interface.l

@@ -5,10 +5,56 @@
 
 (defclass jaxon_jvrc-interface
   :super rtm-ros-robot-interface
-  :slots ())
+  :slots (hand-enable))
 (defmethod jaxon_jvrc-interface
   (:init (&rest args)
-         (send-super* :init :robot jaxon_jvrc-robot args)))
+         (prog1
+             (send-super* :init :robot jaxon_jvrc-robot args)
+           (setq hand-enable (send self :set-hand-controller))
+           (when (and (ros::get-param "/use_sim_time")
+                      (probe-file (ros::resolve-ros-path "package://hrpsys_ros_bridge_jvrc/euslisp/jvrc-hand-interface.l")))
+             (load "package://hrpsys_ros_bridge_jvrc/euslisp/jvrc-hand-interface.l")
+             (send self :put :hand-controller (instance jvrc-hand-controller :init))
+             (setq hand-enable t))
+           ))
+  (:set-hand-controller () nil)
+  (:move-gripper
+   (&rest args)
+   (if hand-enable
+       (send* (send self :get :hand-controller) :move-gripper args)
+     (warn ";; can not use hand~%")))
+  (:start-grasp
+   (&rest args)
+   (if hand-enable
+       (send* (send self :get :hand-controller) :start-grasp args)
+     (warn ";; can not use hand~%")))
+  (:stop-grasp
+   (&rest args)
+   (if hand-enable
+       (send* (send self :get :hand-controller) :stop-grasp args)
+     (warn ";; can not use hand~%")))
+  (:hand-reset
+   (&rest args)
+   (if hand-enable
+       (send* (send self :get :hand-controller) :reset args)
+     (warn ";; can not use hand~%")))
+  ;; (:hand-open
+  ;;  (&rest args)
+  ;;  (send* (send self :get :hand-controller) :open args))
+  ;; (:hand-close
+  ;;  (&rest args)
+  ;;  (send* (send self :get :hand-controller) :close args))
+  (:hand-stop
+   (&rest args)
+   (if hand-enable
+       (send* (send self :get :hand-controller) :stop args)
+     (warn ";; can not use hand~%")))
+  (:hand-resume
+   (&rest args)
+   (if hand-enable
+       (send* (send self :get :hand-controller) :resume args)
+     (warn ";; can not use hand~%")))
+  )
 
 (defun jaxon_jvrc-init (&rest args)
   (if (not (boundp '*ri*))

hrpsys_ros_bridge_jvrc/euslisp/jaxon_jvrc-utils.l

@@ -0,0 +1,28 @@
+(require :jaxon_jvrc "package://hrpsys_ros_bridge_jvrc/models/jaxon_jvrc.l")
+
+(defmethod JAXON_JVRC-robot
+  (:init-ending
+   (&rest args)
+   (prog1
+    (send-super* :init-ending args)
+    (send self :add-thk-contact-coords)
+    ))
+  (:add-thk-contact-coords
+   (&key (offset (float-vector (- 15 12.765) 0 0)))
+   (let* ((limb (list :rarm :larm))
+          (name (list :rhand-contact-coords :lhand-contact-coords))
+          tmpcec)
+     (mapcar #'(lambda (l n sgn)
+                 (setq tmpcec
+                       (make-cascoords
+                        :init :link-list
+                        :parent (send self l :end-coords)
+                        :coords (send (send (send (send self l :end-coords :copy-worldcoords)
+                                                  :translate offset)
+                                            :rotate -pi/2 :y)
+                                      :rotate (* sgn -pi/2) :z)
+                        :name n))
+                 (send self :put n tmpcec)
+                 (send (send self l :end-coords :parent) :assoc (send self :get n)))
+             limb name (list +1 -1))))
+  )