diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt
index 5e2b7745..c1b61b8c 100644
--- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt
+++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt
@@ -277,6 +277,11 @@ if(EXISTS ${hrp2_models_MODEL_DIR}/HRP3HAND_R/HRP3HAND_Rmain.wrl)
HRP3HAND_R)
endif()
+# HIRONX
+compile_openhrp_model_for_closed_robots(HIRONX HIRONX HIRONX
+ --conf-file-option "end_effectors: rarm,RARM_JOINT5,CHEST_JOINT0,-0.05,0,0,0,1,0,1.5708, larm,LARM_JOINT5,CHEST_JOINT0,-0.05,0,0,0,1,0,1.5708,"
+ )
+
# URATALEG
compile_rbrain_model_for_closed_robots(URATALEG URATALEG URATALEG
--robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
@@ -459,6 +464,7 @@ generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP2JSK
generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP2W_for_OpenHRP3 HRP2W "--use-robot-hrpsys-config")
generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP2G_for_OpenHRP3 HRP2G "--use-robot-hrpsys-config")
generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP4R HRP4R "--use-unstable-hrpsys-config")
+generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HIRONX HIRONX "--use-robot-hrpsys-config")
generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(STARO STARO "--use-unstable-hrpsys-config")
generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(JAXON JAXON "--use-robot-hrpsys-config")
if("$ENV{TRAVIS_JOB_ID}" STREQUAL "")
diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l
new file mode 100644
index 00000000..d14e85c2
--- /dev/null
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l
@@ -0,0 +1,183 @@
+(load "package://hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l")
+(require :hironx "package://hrpsys_ros_bridge_tutorials/models/hironx.l")
+(when (probe-file (ros::resolve-ros-path "package://hrpsys_ros_bridge_tutorials/euslisp/hironx-utils.l"))
+ (require :hironx-utils "package://hrpsys_ros_bridge_tutorials/euslisp/hironx-utils.l"))
+(if (ros::resolve-ros-path "package://hironx_ros_bridge")
+ (ros::load-ros-manifest "hironx_ros_bridge"))
+
+(defclass hironx-interface
+ :super rtm-ros-robot-interface
+ :slots ())
+
+;; Initialize
+(defmethod hironx-interface
+ ;; Based on hrp2jsknts-interface.l
+ (:init (&rest args)
+ (prog1
+ (send-super* :init :robot hironx-robot args)
+ ;; add controller
+ (dolist (limb '(:rarm :larm :head :torso))
+ (send self :def-limb-controller-method limb)
+ (send self :add-controller (read-from-string (format nil "~A-controller" limb))
+ :joint-enable-check t :create-actions t))))
+ (:define-all-ROSBridge-srv-methods
+ (&key (debug-view nil) (ros-pkg-name "hrpsys_ros_bridge"))
+ ;; First, define ROSBridge method for old impedance controller
+ (if (ros::resolve-ros-path "package://hironx_ros_bridge")
+ (send-super :define-all-ROSBridge-srv-methods :ros-pkg-name "hironx_ros_bridge"))
+ ;; Second, define ROSBridge method based on hrpsys_ros_bridge
+ ;; Method created already is not overwritten, so we can keep using old impedance controller
+ ;; See :get-ROSBridge-method-def-macro
+ (send-super :define-all-ROSBridge-srv-methods)))
+
+;; ImpedanceControllerService
+;; Based on hironx_client.py in hironx_ros_bridge and rtm-ros-robot-interface.l.
+;; Enable methods executable with old impedance controller and disable others.
+;; The reason why I don't use def-set-get-param-method is that
+;; OpenHRP_ImpedanceControllerService_setImpedanceControllerParam.srv has element "name" inside i_param,
+;; while OpenHRP_ImpedanceControllerService_getImpedanceControllerParam.srv has that element directly.
+;; Set optional-args as (list :name 'name) -> multiple declaration of variable "name" in set-param-method.
+;; Set optional-args as nil -> pass nothing in service call of get-param-method.
+(defmethod hironx-interface
+ (:raw-set-impedance-controller-param (&rest args)
+ (error ";; :raw-set-impedance-controller-param cannot be used with hironx~%"))
+ (:raw-get-impedance-controller-param (&rest args)
+ (error ";; :raw-get-impedance-controller-param cannot be used with hironx~%"))
+ (:start-impedance
+ (limb &rest args &key (m-p 100) (d-p 100) (k-p 100) (m-r 100) (d-r 2000) (k-r 2000)
+ (ref-force #f(0 0 0)) (force-gain #f(1 1 1)) (ref-moment #f(0 0 0))
+ (moment-gain #f(0 0 0)) (sr-gain 1) (avoid-gain 0) (reference-gain 0)
+ (manipulability-limit 0.1))
+ "Start impedance controller mode.
+ limb should be limb symbol name such as :rarm, :larm, or :arms."
+ (let (sensor-name target-name)
+ (cond ((eq limb :rarm)
+ (setq sensor-name "rhsensor" target-name "RARM_JOINT5"))
+ ((eq limb :larm)
+ (setq sensor-name "lhsensor" target-name "LARM_JOINT5"))
+ ((eq limb :arms)
+ (return-from :start-impedance
+ (mapcar #'(lambda (l) (send* self :start-impedance l args))
+ '(:rarm :larm))))
+ (t (error ";; No such limb: ~A~%." limb)))
+ (send self :impedancecontrollerservice_setimpedancecontrollerparam :i_param
+ (instance hironx_ros_bridge::OpenHRP_ImpedanceControllerService_impedanceParam :init
+ :name sensor-name :base_name "CHEST_JOINT0" :target_name target-name
+ :m_p m-p :d_p d-p :k_p k-p :m_r m-r :d_r d-r :k_r k-r :ref_force ref-force
+ :force_gain force-gain :ref_moment ref-moment :moment_gain moment-gain
+ :sr_gain sr-gain :avoid_gain avoid-gain :reference_gain reference-gain
+ :manipulability_limit manipulability-limit))))
+ (:raw-start-impedance (&rest args)
+ (error ";; :raw-start-impedance cannot be used with hironx~%"))
+ (:start-impedance-no-wait (&rest args)
+ (error ";; :start-impedance-no-wait cannot be used with hironx~%"))
+ (:stop-impedance (limb)
+ "Stop impedance controller mode.
+ limb should be limb symbol name such as :rarm, :larm, or :arms."
+ (let (sensor-name)
+ (cond ((eq limb :rarm)
+ (setq sensor-name "rhsensor"))
+ ((eq limb :larm)
+ (setq sensor-name "lhsensor"))
+ ((eq limb :arms)
+ (return-from :stop-impedance
+ (mapcar #'(lambda (l) (send self :stop-impedance l))
+ '(:rarm :larm))))
+ (t (error ";; No such limb: ~A~%." limb)))
+ (send self :impedancecontrollerservice_deleteimpedancecontrollerandwait :name sensor-name)))
+ (:stop-impedance-no-wait (limb)
+ (let (sensor-name)
+ (cond ((eq limb :rarm)
+ (setq sensor-name "rhsensor"))
+ ((eq limb :larm)
+ (setq sensor-name "lhsensor"))
+ ((eq limb :arms)
+ (return-from :stop-impedance-no-wait
+ (mapcar #'(lambda (l) (send self :stop-impedance-no-wait l))
+ '(:rarm :larm))))
+ (t (error ";; No such limb: ~A~%." limb)))
+ (send self :impedancecontrollerservice_deleteimpedancecontroller :name sensor-name)))
+ (:wait-impedance-controller-transition (&rest args)
+ (error ";; :wait-impedance-controller-transition cannot be used with hironx~%"))
+ (:set-impedance-controller-param (&rest args)
+ (error ";; In hironx, we cannot tell :set-impedance-controller-param from :start-impedance~%"))
+ (:get-impedance-controller-param (limb)
+ (let (sensor-name)
+ (cond ((eq limb :rarm)
+ (setq sensor-name "rhsensor"))
+ ((eq limb :larm)
+ (setq sensor-name "lhsensor"))
+ ((eq limb :arms)
+ (return-from :get-impedance-controller-param
+ (mapcar #'(lambda (l) (send self :get-impedance-controller-param l))
+ '(:rarm :larm))))
+ (t (error ";; No such limb: ~A~%." limb)))
+ (send (send self :impedancecontrollerservice_getimpedancecontrollerparam :name sensor-name)
+ :i_param)))
+ (:get-impedance-controller-controller-mode (&rest args)
+ (error ";; :get-impedance-controller-controller-mode cannot be used with hironx~%"))
+ (:force-sensor-method (&rest args)
+ (error ";; :force-sensor-method cannot be used with hironx~%")))
+
+;; ServoControllerService for hand
+;; Based on hironx_client.py in hironx_ros_bridge and hrp2-common-interface.l
+(defmethod hironx-interface
+ (:hand-angle-vector
+ (av &optional (tm 1000) (hand :hands))
+ (let (av-rad-list)
+ ;; Convert deg float-vector (or list) to rad list
+ (dotimes (i (length av))
+ (push (deg2rad (elt av i)) av-rad-list))
+ (setq av-rad-list (reverse av-rad-list))
+ (cond ((eq hand :hands)
+ (send self :servocontrollerservice_setjointangles :jvs av-rad-list :tm (/ tm 1000.0)))
+ ((or (eq hand :rhand) (eq hand :lhand))
+ (send self :servocontrollerservice_setjointanglesofgroup
+ :gname (string-downcase hand) :jvs av-rad-list :tm (/ tm 1000.0)))
+ (t (error ";; No such hand: ~A~%." hand)))))
+ (:hand-servo-on ()
+ (send self :servocontrollerservice_servoon))
+ (:hand-servo-off ()
+ (send self :servocontrollerservice_servooff))
+ (:set-hand-effort (&optional (effort 100))
+ "effort is percentage"
+ (dolist (id (list 2 3 4 5 6 7 8 9))
+ (send self :servocontrollerservice_setmaxtorque :id id :percentage effort)))
+ (:hand-width2angles (width)
+ (let ((safetymargin 3) (l1 41.9) (l2 19) xpos a2pos a1radh a1rad a1deg)
+ (when (or (< width 0) (> width (* (- (+ l1 l2) safetymargin) 2)))
+ (return-from :hand-width2angles nil))
+ (setq xpos (+ (/ width 2.0) safetymargin))
+ (setq a2pos (- xpos l2))
+ (setq a1radh (acos (/ a2pos l1)))
+ (setq a1rad (- (/ pi 2.0) a1radh))
+ (setq a1deg (rad2deg a1rad))
+ (float-vector a1deg (- a1deg) (- a1deg) a1deg)))
+ (:set-hand-width (hand width &key (tm 1000) effort)
+ (when effort
+ (send self :set-hand-effort effort))
+ (send self :hand-angle-vector (send self :hand-width2angles width) tm hand))
+ (:start-grasp (&optional (arm :arms) &key effort)
+ (cond ((eq arm :rarm)
+ (send self :set-hand-width :rhand 0 :effort effort))
+ ((eq arm :larm)
+ (send self :set-hand-width :lhand 0 :effort effort))
+ ((eq arm :arms)
+ (send self :set-hand-width :rhand 0 :effort effort)
+ (send self :set-hand-width :lhand 0 :effort effort))
+ (t (error ";; No such arm: ~A~%." arm))))
+ (:stop-grasp (&optional (arm :arms) &key effort)
+ (cond ((eq arm :rarm)
+ (send self :set-hand-width :rhand 100 :effort effort))
+ ((eq arm :larm)
+ (send self :set-hand-width :lhand 100 :effort effort))
+ ((eq arm :arms)
+ (send self :set-hand-width :rhand 100 :effort effort)
+ (send self :set-hand-width :lhand 100 :effort effort))
+ (t (error ";; No such arm: ~A~%." arm)))))
+
+(defun hironx-init (&rest args)
+ (if (not (boundp '*ri*))
+ (setq *ri* (instance* hironx-interface :init args)))
+ (if (not (boundp '*hironx*))
+ (setq *hironx* (instance hironx-robot :init))))
diff --git a/hrpsys_ros_bridge_tutorials/euslisp/kawada-hironx-example.l b/hrpsys_ros_bridge_tutorials/euslisp/kawada-hironx-example.l
deleted file mode 100644
index 66c7a1be..00000000
--- a/hrpsys_ros_bridge_tutorials/euslisp/kawada-hironx-example.l
+++ /dev/null
@@ -1,17 +0,0 @@
-(ros::roseus "kawadahironx_pickbox")
-(load "package://hrpsys_ros_bridge_tutorials/euslisp/kawada-hironx-interface.l")
-
-(kawada-hironx-init)
-(dotimes (i 10)
- (send *hiro* :reset-pose)
- ;;(objects (list *hiro*))
- (send *ri* :angle-vector (send *hiro* :angle-vector) 3000)
- (send *ri* :wait-interpolation)
- (send *hiro* :rarm :shoulder-p :joint-angle -40)
- (send *hiro* :rarm :elbow-p :joint-angle -130)
- (send *hiro* :head :neck-y :joint-angle -20)
- ;;(objects (list *hiro*))
- (send *ri* :angle-vector (send *hiro* :angle-vector) 3000)
- (send *ri* :wait-interpolation)
- )
-(warning-message 2 ";; type (exit) to finish program~%")
diff --git a/hrpsys_ros_bridge_tutorials/euslisp/kawada-hironx-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/kawada-hironx-interface.l
deleted file mode 100644
index 2467be0e..00000000
--- a/hrpsys_ros_bridge_tutorials/euslisp/kawada-hironx-interface.l
+++ /dev/null
@@ -1,15 +0,0 @@
-(load "package://hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l")
-(require :kawadahironx "package://hrpsys_ros_bridge_tutorials/models/kawada-hironx.l")
-
-(defclass kawadahironx-interface
- :super rtm-ros-robot-interface
- :slots ())
-(defmethod kawadahironx-interface
- (:init (&rest args)
- (send-super :init :robot hironx-robot)))
-
-(defun kawada-hironx-init (&rest args)
- (if (not (boundp '*ri*))
- (setq *ri* (instance* kawadahironx-interface :init args)))
- (if (not (boundp '*hiro*))
- (setq *hiro* (instance hironx-robot :init))))
diff --git a/hrpsys_ros_bridge_tutorials/launch/hironx.launch b/hrpsys_ros_bridge_tutorials/launch/hironx.launch
deleted file mode 100644
index 825acad2..00000000
--- a/hrpsys_ros_bridge_tutorials/launch/hironx.launch
+++ /dev/null
@@ -1,44 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/hrpsys_ros_bridge_tutorials/launch/hironx_nosim.launch b/hrpsys_ros_bridge_tutorials/launch/hironx_nosim.launch
deleted file mode 100644
index 15367dea..00000000
--- a/hrpsys_ros_bridge_tutorials/launch/hironx_nosim.launch
+++ /dev/null
@@ -1,45 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/hrpsys_ros_bridge_tutorials/launch/hironx_ros_bridge.launch b/hrpsys_ros_bridge_tutorials/launch/hironx_ros_bridge.launch
deleted file mode 100644
index 5a50982b..00000000
--- a/hrpsys_ros_bridge_tutorials/launch/hironx_ros_bridge.launch
+++ /dev/null
@@ -1,17 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/hrpsys_ros_bridge_tutorials/launch/hironx_startup.launch b/hrpsys_ros_bridge_tutorials/launch/hironx_startup.launch
deleted file mode 100644
index 1c63f32e..00000000
--- a/hrpsys_ros_bridge_tutorials/launch/hironx_startup.launch
+++ /dev/null
@@ -1,20 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/hrpsys_ros_bridge_tutorials/models/kawada-hironx.yaml b/hrpsys_ros_bridge_tutorials/models/hironx.yaml
similarity index 84%
rename from hrpsys_ros_bridge_tutorials/models/kawada-hironx.yaml
rename to hrpsys_ros_bridge_tutorials/models/hironx.yaml
index 130fce33..e0e6d122 100644
--- a/hrpsys_ros_bridge_tutorials/models/kawada-hironx.yaml
+++ b/hrpsys_ros_bridge_tutorials/models/hironx.yaml
@@ -41,6 +41,5 @@ head-end-coords:
angle-vector:
reset-pose : [0.0, -40.0, -90.0, 0.0, 0.0, 0.0, 0.0, -40.0, -90.0, 0.0, 0.0, 0.0, 0.0, 0.0, 30.0]
off-pose : [0.0,-140.0,-158.0, 0.0, 0.0, 0.0, 0.0,-140.0,-158.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-
-
-
+ ## _InitialPose in hironx_client.py in hironx_ros_bridge (used in goInitial())
+ reset-manip-pose : [-0.6, 0.0, -100.0, 15.2, 9.4, 3.2, 0.6, 0.0, -100.0, -15.2, 9.4, -3.2, 0.0, 0.0, 0.0]