From d92bb293da63938cbfa47cc1fd372bab03fbdabc Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Wed, 26 Aug 2015 16:42:54 +0900 Subject: [PATCH 1/4] [hrpsys_ros_bridge/test/hrpsys-samples, hrpsys_ros_bridge/models/samplerobot.yaml, hrpsys_ros_bridge/euslisp/samplerobot-interface.l] Move samplerobot euslisp tests and setting files from hrpsys_ros_bridge_tutorials. --- .../euslisp/samplerobot-interface.l | 67 +++ hrpsys_ros_bridge/models/samplerobot.yaml | 101 +++++ .../samplerobot-auto-balancer.l | 427 ++++++++++++++++++ .../samplerobot-collision-detector.l | 63 +++ .../hrpsys-samples/samplerobot-data-logger.l | 42 ++ .../samplerobot-impedance-controller.l | 55 +++ .../samplerobot-remove-force-offset.l | 63 +++ .../samplerobot-sequence-player.l | 37 ++ .../hrpsys-samples/samplerobot-stabilizer.l | 51 +++ .../hrpsys-samples/samplerobot-terrain-walk.l | 100 ++++ .../hrpsys-samples/samplerobot-unittest.l | 32 ++ .../test_samplerobot_euslisp_unittests.launch | 23 + 12 files changed, 1061 insertions(+) create mode 100644 hrpsys_ros_bridge/euslisp/samplerobot-interface.l create mode 100644 hrpsys_ros_bridge/models/samplerobot.yaml create mode 100644 hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-auto-balancer.l create mode 100644 hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-collision-detector.l create mode 100644 hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-data-logger.l create mode 100644 hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-impedance-controller.l create mode 100644 hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-remove-force-offset.l create mode 100644 hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-sequence-player.l create mode 100644 hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-stabilizer.l create mode 100644 hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-terrain-walk.l create mode 100644 hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-unittest.l create mode 100644 hrpsys_ros_bridge/test/hrpsys-samples/test_samplerobot_euslisp_unittests.launch diff --git a/hrpsys_ros_bridge/euslisp/samplerobot-interface.l b/hrpsys_ros_bridge/euslisp/samplerobot-interface.l new file mode 100644 index 000000000..60ef26463 --- /dev/null +++ b/hrpsys_ros_bridge/euslisp/samplerobot-interface.l @@ -0,0 +1,67 @@ +(load "package://hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l") +(require :samplerobot "package://hrpsys_ros_bridge/models/samplerobot.l") + +(defclass samplerobot-interface + :super rtm-ros-robot-interface + :slots ()) +(defmethod samplerobot-interface + (:init (&rest args) + (send-super* :init :robot samplerobot-robot args) + (mapcar #'(lambda (ctype) + (send self :add-controller ctype)) + (send self :default-controller-list)) + ) + (:default-controller-list () + (list :larm-controller + :rarm-controller + :torso-controller + :lhand-controller + :rhand-controller + :fullbody-controller)) + (:default-controller () + (mapcar #'(lambda (ctype) (car (send self ctype))) (send self :default-controller-list))) + (:fullbody-controller () + (send-message self robot-interface :default-controller)) + (:larm-controller () + (list + (list + (cons :controller-action "/larm_controller/follow_joint_trajectory_action") + (cons :controller-state "/larm_controller/state") + (cons :action-type control_msgs::FollowJointTrajectoryAction) + (cons :joint-names (list "LARM_SHOULDER_P" "LARM_SHOULDER_R" "LARM_SHOULDER_Y" "LARM_ELBOW" "LARM_WRIST_Y" "LARM_WRIST_P"))))) + (:rarm-controller () + (list + (list + (cons :controller-action "/rarm_controller/follow_joint_trajectory_action") + (cons :controller-state "/rarm_controller/state") + (cons :action-type control_msgs::FollowJointTrajectoryAction) + (cons :joint-names (list "RARM_SHOULDER_P" "RARM_SHOULDER_R" "RARM_SHOULDER_Y" "RARM_ELBOW" "RARM_WRIST_Y" "RARM_WRIST_P"))))) + (:torso-controller () + (list + (list + (cons :controller-action "/torso_controller/follow_joint_trajectory_action") + (cons :controller-state "/torso_controller/state") + (cons :action-type control_msgs::FollowJointTrajectoryAction) + (cons :joint-names (list "WAIST_P" "WAIST_R" "CHEST"))))) + (:lhand-controller () + (list + (list + (cons :controller-action "/lhand_controller/follow_joint_trajectory_action") + (cons :controller-state "/lhand_controller/state") + (cons :action-type control_msgs::FollowJointTrajectoryAction) + (cons :joint-names (list "LARM_WRIST_R"))))) + (:rhand-controller () + (list + (list + (cons :controller-action "/rhand_controller/follow_joint_trajectory_action") + (cons :controller-state "/rhand_controller/state") + (cons :action-type control_msgs::FollowJointTrajectoryAction) + (cons :joint-names (list "RARM_WRIST_R"))))) + ) + +(defun samplerobot-init (&rest args) + (if (not (boundp '*ri*)) + (setq *ri* (instance* samplerobot-interface :init args))) + (if (not (boundp '*sr*)) + (setq *sr* (instance samplerobot-robot :init))) + ) diff --git a/hrpsys_ros_bridge/models/samplerobot.yaml b/hrpsys_ros_bridge/models/samplerobot.yaml new file mode 100644 index 000000000..e3f6599f5 --- /dev/null +++ b/hrpsys_ros_bridge/models/samplerobot.yaml @@ -0,0 +1,101 @@ +## +## - collada_joint_name : euslisp_joint_name (start with :) +## + +rleg: + - RLEG_HIP_R : rleg-crotch-r + - RLEG_HIP_P : rleg-crotch-p + - RLEG_HIP_Y : rleg-crotch-y + - RLEG_KNEE : rleg-knee-p + - RLEG_ANKLE_P : rleg-ankle-p + - RLEG_ANKLE_R : rleg-ankle-r +rarm: + - RARM_SHOULDER_P : rarm-shoulder-p + - RARM_SHOULDER_R : rarm-shoulder-r + - RARM_SHOULDER_Y : rarm-shoulder-y + - RARM_ELBOW : rarm-elbow-p + - RARM_WRIST_Y : rarm-wrist-y + - RARM_WRIST_P : rarm-wrist-p + - RARM_WRIST_R : rarm-wrist-r +lleg: + - LLEG_HIP_R : lleg-crotch-r + - LLEG_HIP_P : lleg-crotch-p + - LLEG_HIP_Y : lleg-crotch-y + - LLEG_KNEE : lleg-knee-p + - LLEG_ANKLE_P : lleg-ankle-p + - LLEG_ANKLE_R : lleg-ankle-r +larm: + - LARM_SHOULDER_P : larm-shoulder-p + - LARM_SHOULDER_R : larm-shoulder-r + - LARM_SHOULDER_Y : larm-shoulder-y + - LARM_ELBOW : larm-elbow-p + - LARM_WRIST_Y : larm-wrist-y + - LARM_WRIST_P : larm-wrist-p + - LARM_WRIST_R : larm-wrist-r +torso: + - WAIST_P : torso-waist-p + - WAIST_R : torso-waist-r + - CHEST : torso-waist-y + +## +## end-coords +## +larm-end-coords: + translate : [0, 0, -0.12] + rotate : [0, 1, 0, 90] + parent : LARM_LINK6 +rarm-end-coords: + translate : [0, 0, -0.12] + rotate : [0, 1, 0, 90] + parent : RARM_LINK6 +lleg-end-coords: + translate : [0.00, 0, -0.07] +rleg-end-coords: + translate : [0.00, 0, -0.07] +head-end-coords: + translate : [0.08, 0, 0.13] + rotate : [0, 1, 0, 90] + +## +## reset-pose +## +angle-vector: +# old reset-pose +# reset-pose : [0.0, -20.0, 0.0, 47.0, -27.0, 0.0, # rleg +# 30.0, 0.0, 0.0, -60.0, 9.0, -6.5, 36.5, # rarm +# 0.0, -20.0, 0.0, 47.0, -27.0, 0.0, # lleg +# 30.0, 0.0, 0.0, -60.0, -9.0, -6.5, -36.5, # larm +# 0.0, 0.0, 0.0] # torso + # openhrp reset pose from the initial line of OpenHRP-3.1/sample/controller/SampleController/etc/Sample.pos + reset-pose : [-0.004457, -21.6929, -0.01202, 47.6723, -25.93, 0.014025, # rleg + 17.8356, -9.13759, -6.61188, -36.456, 0.0, 0.0, 0.0, # rarm + -0.004457, -21.6929, -0.01202, 47.6723, -25.93, 0.014025, # lleg + 17.8356, 9.13759, 6.61188, -36.456, 0.0, 0.0, 0.0, # larm + 0.0, 0.0, 0.0] # torso + reset-manip-pose : [0.0, -20.0, 0.0, 47.0, -27.0, 0.0, # rleg + 30.0, 0.0, 0.0, -100.0, 9.0, -6.5, 36.5, # rarm + 0.0, -20.0, 0.0, 47.0, -27.0, 0.0, # lleg + 30.0, 0.0, 0.0, -100.0, -9.0, -6.5, -36.5, # larm + 0.0, 0.0, 0.0] # torso + + +# for gazebo simulation +replace_xmls: + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: LLEG_LINK6 + replaced_xml: '\n 1000000.0\n 100.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: RLEG_LINK6 + replaced_xml: '\n 1000000.0\n 100.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + attribute_name: effort + attribute_value: 100 + replaced_attribute_value: 200 + - match_rule: + attribute_name: velocity + attribute_value: 0.5 + replaced_attribute_value: 6.0 diff --git a/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-auto-balancer.l b/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-auto-balancer.l new file mode 100644 index 000000000..7e06e5a07 --- /dev/null +++ b/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-auto-balancer.l @@ -0,0 +1,427 @@ +#!/usr/bin/env roseus + +;; $ rtmlaunch hrpsys_ros_bridge samplerobot.launch +;; $ roseus samplerobot-auto-balancer.l "(samplerobot-auto-balancer-demo)" + +(load "package://hrpsys_ros_bridge/euslisp/samplerobot-interface.l") + +(defun samplerobot-auto-balancer-init () + (samplerobot-init) + (initialize-pose-list) + t) + +(defun test-Pose-List (pose-list initial-pose) + (dolist (pose pose-list) + (send *sr* :angle-vector pose) + (send *ri* :angle-vector (send *sr* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (send *sr* :angle-vector initial-pose) + (send *ri* :angle-vector (send *sr* :angle-vector) 1000) + (send *ri* :wait-interpolation))) + +(defun initialize-pose-list + () + (setq *initial-pose* (send *sr* :reset-pose)) + (send *sr* :arms :shoulder-p :joint-angle -90) + (send *sr* :arms :elbow-p :joint-angle -20) + (setq *arm-front-pose* (send *sr* :angle-vector)) + (send *sr* :reset-pose) + (send *sr* :legs :move-end-pos #f(0 0 70)) + (setq *half-sitting-pose* (send *sr* :angle-vector)) + (send *sr* :reset-pose) + (send *sr* :fix-leg-to-coords (make-coords)) + (send *sr* :legs :move-end-pos #f(0 0 70)) + (let ((lc (send *sr* :legs :end-coords :copy-worldcoords))) + (send *sr* :move-coords (send (send (car (send *sr* :links)) :copy-worldcoords) :rotate (deg2rad 10) :x) (car (send *sr* :links))) + (mapcar #'(lambda (l c) (send *sr* l :inverse-kinematics c)) '(:lleg :rleg) lc) + (send *sr* :move-centroid-on-foot :both '(:rleg :lleg)) + (setq *root-rot-x-pose* (send *sr* :angle-vector))) + (send *sr* :reset-pose) + (send *sr* :fix-leg-to-coords (make-coords)) + (send *sr* :legs :move-end-pos #f(0 0 70)) + (let ((lc (send *sr* :legs :end-coords :copy-worldcoords))) + (send *sr* :move-coords (send (send (car (send *sr* :links)) :copy-worldcoords) :rotate (deg2rad 20) :y) (car (send *sr* :links))) + (mapcar #'(lambda (l c) (send *sr* l :inverse-kinematics c)) '(:lleg :rleg) lc) + (send *sr* :move-centroid-on-foot :both '(:rleg :lleg)) + (setq *root-rot-y-pose* (send *sr* :angle-vector))) + (setq *pose-list* (list *half-sitting-pose* *root-rot-x-pose* *root-rot-y-pose*)) + (send *sr* :angle-vector *initial-pose*) + (send *ri* :angle-vector (send *sr* :angle-vector) 2000) + (send *ri* :wait-interpolation) + ) + +(defun samplerobot-auto-balancer-demo0 () + (print "1. AutoBalancer mode by fixing feet") + (send *ri* :start-auto-balancer) + (test-pose-list (list *arm-front-pose*) *initial-pose*) + (send *ri* :stop-auto-balancer) + (print " Start and Stop AutoBalancer by fixing feet=>OK") + t) + +(defun samplerobot-auto-balancer-demo1 () + (print "2. AutoBalancer mode by fixing hands and feet") + (send *ri* :start-auto-balancer :limbs '(:rleg :lleg :rarm :larm)) + (test-pose-list (list *arm-front-pose*) *initial-pose*) + (send *ri* :stop-auto-balancer) + (print " Start and Stop AutoBalancer by fixing hands and feet=>OK") + t) + +(defun samplerobot-auto-balancer-demo2 () + (print "3. getAutoBalancerParam") + (pprint (send (send *ri* :get-auto-balancer-param) :slots)) + (print " getAutoBalancerParam() => OK") + t) + +(defun samplerobot-auto-balancer-demo3 () + (print "4. setAutoBalancerParam") + (let ((zmpoff (list (float-vector 100 0 0) (float-vector 100 0 0)))) + (send *ri* :set-auto-balancer-param :default-zmp-offsets zmpoff) + (let ((dd (send (send (send *ri* :get-auto-balancer-param) :default_zmp_offsets) :data))) + (if (and (eps-v= (car zmpoff) (scale 1e3 (subseq dd 0 3))) (eps-v= (cadr zmpoff) (scale 1e3 (subseq dd 3)))) ;; [m] => [mm] + (print " setAutoBalancerParam() => OK")))) + (print " default_zmp_offsets setting check in start and stop") + (send *ri* :start-auto-balancer) + (send *ri* :stop-auto-balancer) + (send *ri* :set-auto-balancer-param :default-zmp-offsets (list (float-vector 0 0 0) (float-vector 0 0 0))) + t) + +(defun samplerobot-auto-balancer-demo4 () + (print "5. change base height, base rot x, base rot y, and upper body while AutoBalancer mode") + (send *ri* :start-auto-balancer) + (test-pose-list *pose-list* *initial-pose*) + (send *ri* :stop-auto-balancer) + ) + +(defun samplerobot-auto-balancer-demo5 () + (print "6. start stop check") + (send *ri* :set-auto-balancer-param :default-zmp-offsets (list (float-vector -50 50 0) (float-vector -50 50 0))) + (dolist (pose *pose-list*) + (send *sr* :angle-vector pose) + (send *ri* :angle-vector (send *sr* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (send *ri* :start-auto-balancer) + (send *ri* :stop-auto-balancer) + ) + (send *ri* :set-auto-balancer-param :default-zmp-offsets (list (float-vector 0 0 0) (float-vector 0 0 0))) + (send *ri* :angle-vector (send *sr* :angle-vector) 1000) + (send *ri* :wait-interpolation) + ) + +(defun samplerobot-auto-balancer-demo6 () + (print "7. balance against hand force") + (send *ri* :start-auto-balancer) + (send *ri* :set-ref-force (float-vector 0 0 -50) 1000 :rarm) + (send *ri* :wait-interpolation-seq) + (send *ri* :set-ref-force (float-vector 0 0 0) 1000 :rarm) + (send *ri* :wait-interpolation-seq) + (send *ri* :stop-auto-balancer) + ) + +(defun samplerobot-auto-balancer-demo7 () + (print "1. goPos") + (send *ri* :go-pos 0.1 0.05 20) + (print " goPos()=>OK") + t) + +(defun samplerobot-auto-balancer-demo8 () + (print "2. goVelocity and goStop") + (send *ri* :go-velocity -0.1 -0.05 -20) + (unix:sleep 1) + (send *ri* :go-stop) + (print " goVelocity()=>OK") + t) + +(defun samplerobot-auto-balancer-demo9 () + (print "3. setFootSteps") + (send *ri* :set-foot-steps + (list (make-coords :pos (float-vector 0 -90 0) :name :rleg) + (make-coords :pos (float-vector 0 90 0) :name :lleg))) + (send *ri* :wait-foot-steps) + (send *ri* :set-foot-steps + (list (make-coords :pos (float-vector 0 -90 0) :name :rleg) + (make-coords :pos (float-vector 150 90 0) :name :lleg) + (make-coords :pos (float-vector 300 -90 0) :name :rleg) + (make-coords :pos (float-vector 300 90 0) :name :lleg))) + (send *ri* :wait-foot-steps) + (print " setFootSteps()=>OK") + t) + +(defun samplerobot-auto-balancer-demo10 () + (print "4. change base height, base rot x, base rot y, and upper body while walking") + (send *ri* :start-auto-balancer) + (send *ri* :go-velocity 0 0 0) + (test-pose-list *pose-list* *initial-pose*) + (send *ri* :go-stop) + (send *ri* :stop-auto-balancer) + t) + +(defun samplerobot-auto-balancer-demo11 () + (print "5. getGaitGeneratorParam") + (pprint (send (send *ri* :get-gait-generator-param) :slots)) + (print " getGaitGeneratorParam() => OK") + t) + +(defun samplerobot-auto-balancer-demo12 () + (print "6. setGaitGeneratorParam") + (let ((org-gp (send *ri* :get-gait-generator-param)) + (default-step-time 0.7) + (default-step-height 0.15) + (default-double-support-ratio 0.4) + (default-orbit-type :RECTANGLE)) + (send *ri* :set-gait-generator-param + :default-step-time default-step-time + :default-step-height default-step-height + :default-double-support-ratio default-double-support-ratio + :default-orbit-type default-orbit-type) + (let ((gp (send *ri* :get-gait-generator-param))) + (if (and (eps= (send gp :default_step_time) default-step-time) + (eps= (send gp :default_step_height) default-step-height) + (eps= (send gp :default_double_support_ratio) default-double-support-ratio) + (eq (send gp :default_orbit_type) 2)) ;; rectangle + (print " setGaitGeneratorParam() => OK"))) + (send *ri* :go-velocity 0.1 0 0) + (unix:sleep 1) + (send *ri* :go-stop) + (send *ri* :set-gait-generator-param + :default-step-time (send org-gp :default_step_time) + :default-step-height (send org-gp :default_step_height) + :default-double-support-ratio (send org-gp :default_double_support_ratio) + :default-orbit-type (send org-gp :default_orbit_type) + ) + t)) + +(defun samplerobot-auto-balancer-demo13 () + (print "7. non-default stride") + (send *ri* :start-auto-balancer) + (send *ri* :set-foot-steps + (list (make-coords :pos (float-vector 0 -90 0) :name :rleg) + (make-coords :pos (float-vector 150 90 0) :name :lleg))) + (send *ri* :wait-foot-steps) + (send *ri* :set-foot-steps + (list (make-coords :pos (float-vector 0 -90 0) :name :rleg) + (make-coords :pos (float-vector 0 90 0) :name :lleg))) + (send *ri* :wait-foot-steps) + (send *ri* :stop-auto-balancer) + (print " Non default Stride()=>OK") + t) + +(defun samplerobot-auto-balancer-demo14 () + (print "8. Use toe heel contact") + (send *ri* :start-auto-balancer) + (send *ri* :set-gait-generator-param + :toe-pos-offset-x (* 1e-3 182.0) :heel-pos-offset-x (* 1e-3 -72.0) + :toe-zmp-offset-x (* 1e-3 182.0) :heel-zmp-offset-x (* 1e-3 -72.0) + :toe-angle 20 :heel-angle 10) + (send *ri* :go-pos 0.3 0 0) + (send *ri* :stop-auto-balancer) + (send *ri* :set-gait-generator-param + :toe-angle 0 :heel-angle 0) + (print " Toe heel contact=>OK") + t) + +(defun samplerobot-auto-balancer-demo15 () + (print "9. Stop and start auto balancer sync check2") + t) + +(defun samplerobot-auto-balancer-demo16 () + (print "10. Emergency stop") + (send *ri* :start-auto-balancer) + (send *ri* :go-pos-no-wait 0 0 90) + (print " Start goPos and wait for 4 steps") + (dotimes (i 4) ;; Wait for 4 steps including initial double support phase + ;; Wait for 1 steps + (send *ri* :angle-vector (send *sr* :angle-vector) (* 1e3 (send (send *ri* :get-gait-generator-param) :default_step_time))) + (send *ri* :wait-interpolation)) + (print " Emergency stoping") + (send *ri* :emergency-walking-stop) + (print " Align foot steps") + (send *ri* :go-pos 0 0 0) + t) + +(defun samplerobot-auto-balancer-demo17 () + (print "11. Get remaining foot steps") + (send *sr* :fix-leg-to-coords (send *rI* :get-foot-step-param :dst-foot-midcoords)) + (objects (list *sr*)) + (send *ri* :go-pos-no-wait 0.3 0.1 15) + (let ((fs-list (send *ri* :get-remaining-foot-step-sequence))) + (while fs-list + (setq fs-list (send *ri* :get-remaining-foot-step-sequence)) + (send *irtviewer* :draw-objects :flush nil) + (send *ri* :draw-remaining-foot-step-sequence (send *irtviewer* :viewer) :flush t) + (send *ri* :angle-vector (send *sr* :angle-vector) (* 1e3 (send (send *ri* :get-gait-generator-param) :default_step_time))) + (send *ri* :wait-interpolation)) + t)) + +(defun samplerobot-auto-balancer-demo18 () + (print "12. Change step param with setFootSteps") + (send *ri* :get-gait-generator-param) + ;; Dummy settting just for testing + (send *ri* :set-gait-generator-param :toe-angle 50 :heel-angle 50 :default-step-time 4.0 :default-step-height 0.0) + ;; Step time change + (send *ri* :set-foot-steps-with-param + (list (make-coords :coords (send *sr* :rleg :end-coords :copy-worldcoords) :name :rleg) + (make-coords :coords (send (send *sr* :lleg :end-coords :copy-worldcoords) :translate (float-vector 100 0 0)) :name :lleg) + (make-coords :coords (send (send *sr* :rleg :end-coords :copy-worldcoords) :translate (float-vector 200 0 0)) :name :rleg) + (make-coords :coords (send (send *sr* :lleg :end-coords :copy-worldcoords) :translate (float-vector 200 0 0)) :name :lleg)) + (list 0 50 50 50) + (list 1.0 2.0 1.0 2.0) + (list 0 0 0 0) + (list 0 0 0 0)) + ;; Step height change + (send *ri* :set-foot-steps-with-param + (list (make-coords :coords (send *sr* :rleg :end-coords :copy-worldcoords) :name :rleg) + (make-coords :coords (send (send *sr* :lleg :end-coords :copy-worldcoords) :translate (float-vector 100 0 0)) :name :lleg) + (make-coords :coords (send (send *sr* :rleg :end-coords :copy-worldcoords) :translate (float-vector 200 0 0)) :name :rleg) + (make-coords :coords (send (send *sr* :lleg :end-coords :copy-worldcoords) :translate (float-vector 200 0 0)) :name :lleg)) + (list 0 100 50 100) + (list 1.0 1.0 1.0 1.0) + (list 0 0 0 0) + (list 0 0 0 0)) + ;; Toe heel change + (send *ri* :set-foot-steps-with-param + (list (make-coords :coords (send *sr* :rleg :end-coords :copy-worldcoords) :name :rleg) + (make-coords :coords (send (send *sr* :lleg :end-coords :copy-worldcoords) :translate (float-vector 100 0 0)) :name :lleg) + (make-coords :coords (send (send *sr* :rleg :end-coords :copy-worldcoords) :translate (float-vector 200 0 0)) :name :rleg) + (make-coords :coords (send (send *sr* :lleg :end-coords :copy-worldcoords) :translate (float-vector 200 0 0)) :name :lleg)) + (list 0 50 50 50) + (list 1.0 1.0 1.0 1.0) + (list 0 0 20 10) + (list 0 0 50 10)) + (send *ri* :set-gait-generator-param :toe-angle 0 :heel-angle 0 :default-step-time 1.0 :default-step-height 0.05) + ) + +(defun samplerobot-auto-balancer-demo19 () + (print "13. Overwrite footsteps during walking.") + (send *ri* :set-foot-steps-no-wait + (list (make-coords :coords (send *sr* :rleg :end-coords :copy-worldcoords) :name :rleg) + (make-coords :coords (send (send *sr* :lleg :end-coords :copy-worldcoords) :translate (float-vector 100 0 0)) :name :lleg) + (make-coords :coords (send (send *sr* :rleg :end-coords :copy-worldcoords) :translate (float-vector 200 0 0)) :name :rleg) + (make-coords :coords (send (send *sr* :lleg :end-coords :copy-worldcoords) :translate (float-vector 200 100 0)) :name :lleg) + (make-coords :coords (send (send *sr* :rleg :end-coords :copy-worldcoords) :translate (float-vector 200 100 0)) :name :rleg) + (make-coords :coords (send (send *sr* :lleg :end-coords :copy-worldcoords) :translate (float-vector 200 100 0)) :name :lleg) + )) + ;; used for waiting + (send *ri* :angle-vector (send *sr* :angle-vector) 2000) + (send *ri* :wait-interpolation) + ;; overwriting + (send *ri* :set-foot-steps-no-wait + (list (make-coords :coords (send *sr* :rleg :end-coords :copy-worldcoords) :name :rleg) + (make-coords :coords (send (send *sr* :lleg :end-coords :copy-worldcoords) :translate (float-vector 100 0 0)) :name :lleg) + (make-coords :coords (send (send *sr* :rleg :end-coords :copy-worldcoords) :translate (float-vector 200 0 0)) :name :rleg) + (make-coords :coords (send (send *sr* :lleg :end-coords :copy-worldcoords) :translate (float-vector 200 100 0)) :name :lleg) + (make-coords :coords (send (send *sr* :rleg :end-coords :copy-worldcoords) :translate (float-vector 200 100 0)) :name :rleg) + (make-coords :coords (send (send *sr* :lleg :end-coords :copy-worldcoords) :translate (float-vector 200 100 0)) :name :lleg) + )) + ;; used for waiting + (send *ri* :angle-vector (send *sr* :angle-vector) 2000) + (send *ri* :wait-interpolation) + ;; overwrite + (send *ri* :set-foot-steps-no-wait + (list (make-coords :coords (send *sr* :rleg :end-coords :copy-worldcoords) :name :rleg) + (make-coords :coords (send (send *sr* :lleg :end-coords :copy-worldcoords) :translate (float-vector 100 0 0)) :name :lleg) + (make-coords :coords (send (send *sr* :rleg :end-coords :copy-worldcoords) :translate (float-vector 200 0 0)) :name :rleg) + (make-coords :coords (send (send *sr* :lleg :end-coords :copy-worldcoords) :translate (float-vector 200 100 0)) :name :lleg) + (make-coords :coords (send (send *sr* :rleg :end-coords :copy-worldcoords) :translate (float-vector 200 100 0)) :name :rleg) + (make-coords :coords (send (send *sr* :lleg :end-coords :copy-worldcoords) :translate (float-vector 200 100 0)) :name :lleg) + )) + (send *ri* :wait-foot-steps) + ;; memo + ;; Current support leg is obtained by (send *ri* :get-foot-step-param :support-leg) and current support leg coords is obtained by (send *ri* :get-foot-step-param :support-leg-coords) + ;; In normal case, next support leg is obtained (case (send *ri* :get-foot-step-param :support-leg) (:rleg :lleg) (:lleg :rleg)) and next support leg coords is obtained by (send *ri* :get-foot-step-param :swing-leg-dst-coords) + ) + +(defun samplerobot-auto-balancer-demo19 (&key (overwrite-offset-idx 1)) + (print "13. Overwrite footsteps during walking.") + (send *ri* :start-auto-balancer) + (samplerobot-auto-balancer-demo19-base :x overwrite-offset-idx t) + (send *ri* :angle-vector (send *sr* :angle-vector) (* 1000 overwrite-offset-idx)) + (send *ri* :wait-interpolation) + (samplerobot-auto-balancer-demo19-base :y overwrite-offset-idx t) + (send *ri* :angle-vector (send *sr* :angle-vector) (* 1000 overwrite-offset-idx)) + (send *ri* :wait-interpolation) + (samplerobot-auto-balancer-demo19-base :x overwrite-offset-idx t) + (send *ri* :angle-vector (send *sr* :angle-vector) (* 1000 overwrite-offset-idx)) + (send *ri* :wait-interpolation) + (send *ri* :wait-foot-steps) + ) + +(defun samplerobot-auto-balancer-demo19-base (axis overwrite-offset-idx init-fs) + (when init-fs + (send *ri* :set-foot-steps-no-wait + (list (make-coords :coords (send *sr* :rleg :end-coords :copy-worldcoords) :name :rleg) + (make-coords :coords (send (send *sr* :lleg :end-coords :copy-worldcoords) :translate (float-vector 100 0 0)) :name :lleg) + (make-coords :coords (send (send *sr* :rleg :end-coords :copy-worldcoords) :translate (float-vector 200 0 0)) :name :rleg) + (make-coords :coords (send (send *sr* :lleg :end-coords :copy-worldcoords) :translate (float-vector 300 0 0)) :name :lleg) + (make-coords :coords (send (send *sr* :rleg :end-coords :copy-worldcoords) :translate (float-vector 400 0 0)) :name :rleg) + (make-coords :coords (send (send *sr* :lleg :end-coords :copy-worldcoords) :translate (float-vector 400 0 0)) :name :lleg) + ))) + (print ";; Overwrite footsteps") + (let* ((ret (send *ri* :get-remaining-foot-step-sequence-current-index)) + (remain-fs (car ret)) + (current-index (cadr ret))) + (format t ";; Remaining legs = ~A~%" (send-all remain-fs :name)) + (format t ";; Remaining idx = ~A~%" (let ((idx -1)) (mapcar #'(lambda (x) (+ current-index (incf idx))) remain-fs))) + (let* ((overwrite-footstep-index (+ current-index overwrite-offset-idx)) + (support-fs (elt remain-fs (- overwrite-offset-idx 1))) ;; support fs before ovewritten fs + (pos-offset1) (pos-offset2)) + (format t ";; Overwrite index = ~A, leg = ~A, current index = ~A~%" + overwrite-footstep-index (send (elt remain-fs overwrite-offset-idx) :name) + current-index) + (case axis + (:x + (setq pos-offset1 (float-vector 100 0 0) pos-offset2 (float-vector 200 0 0))) + (t + (setq pos-offset1 (float-vector 0 (case (send support-fs :name) (:rleg 100) (:lleg -100)) 0) + pos-offset2 pos-offset1))) + (let ((new-foot-steps + (list + (make-coords :coords (send support-fs :copy-worldcoords) :name (send support-fs :name)) + (make-coords :coords (send (send support-fs :copy-worldcoords) + :translate (v+ pos-offset1 (float-vector 0 (case (send support-fs :name) (:rleg (* 2 90)) (:lleg (* 2 -90))) 0))) + :name (case (send support-fs :name) (:rleg :lleg) (:lleg :rleg))) + (make-coords :coords (send (send support-fs :copy-worldcoords) + :translate pos-offset1) + :name (send support-fs :name)) + (make-coords :coords (send (send support-fs :copy-worldcoords) + :translate (v+ pos-offset2 (float-vector 0 (case (send support-fs :name) (:rleg (* 2 90)) (:lleg (* 2 -90))) 0))) + :name (case (send support-fs :name) (:rleg :lleg) (:lleg :rleg))) + (make-coords :coords (send (send support-fs :copy-worldcoords) + :translate pos-offset2) + :name (send support-fs :name))))) + (send *ri* :set-foot-steps-no-wait + new-foot-steps :overwrite-footstep-index overwrite-footstep-index)))) + ;; memo + ;; Current support leg is obtained by (send *ri* :get-foot-step-param :support-leg) and current support leg coords is obtained by (send *ri* :get-foot-step-param :support-leg-coords) + ;; In normal case, next support leg is obtained (case (send *ri* :get-foot-step-param :support-leg) (:rleg :lleg) (:lleg :rleg)) and next support leg coords is obtained by (send *ri* :get-foot-step-param :swing-leg-dst-coords) + ) + +(defun samplerobot-auto-balancer-demo () + (samplerobot-auto-balancer-init) + + ;; sample for AutoBalancer mode + (samplerobot-auto-balancer-demo0) + (samplerobot-auto-balancer-demo1) + (samplerobot-auto-balancer-demo2) + (samplerobot-auto-balancer-demo3) + (samplerobot-auto-balancer-demo4) + (samplerobot-auto-balancer-demo5) + (samplerobot-auto-balancer-demo6) + + ;; sample for walk pattern generation by AutoBalancer RTC + (samplerobot-auto-balancer-demo7) + (samplerobot-auto-balancer-demo8) + (samplerobot-auto-balancer-demo9) + (samplerobot-auto-balancer-demo10) + (samplerobot-auto-balancer-demo11) + (samplerobot-auto-balancer-demo12) + (samplerobot-auto-balancer-demo13) + (samplerobot-auto-balancer-demo14) + (samplerobot-auto-balancer-demo15) + (samplerobot-auto-balancer-demo16) + (samplerobot-auto-balancer-demo17) + (samplerobot-auto-balancer-demo18) + (samplerobot-auto-balancer-demo19) + (samplerobot-auto-balancer-demo19 :overwrite-offset-idx 2) + ) + +(warn ";; (samplerobot-auto-balancer-demo)~%") diff --git a/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-collision-detector.l b/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-collision-detector.l new file mode 100644 index 000000000..5cd067554 --- /dev/null +++ b/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-collision-detector.l @@ -0,0 +1,63 @@ +#!/usr/bin/env roseus + +;; $ rtmlaunch hrpsys_ros_bridge samplerobot.launch +;; $ roseus samplerobot-collision-detector.l "(samplerobot-collision-detector-demo)" + +(load "package://hrpsys_ros_bridge/euslisp/samplerobot-interface.l") + +(defun samplerobot-collision-detector-init () + (samplerobot-init) + (send *sr* :reset-pose) + (send *ri* :angle-vector (send *sr* :angle-vector) 2000) + (send *ri* :wait-interpolation) + t) + +(defun samplerobot-collision-detector-demo0 () + "1. CollisionCheck in safe pose" + (send *sr* :reset-pose) + (send *ri* :angle-vector (send *sr* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (if (send (send *ri* :get-collision-status) :safe_posture) + (print "Safe pose")) + t) + +(defun samplerobot-collision-detector-demo1 () + "2. CollisionCheck in fail pose" + (send *sr* :reset-pose) + (send *sr* :rarm :move-end-pos (float-vector -50 150 50) :world :rotation-axis nil) + (send *ri* :angle-vector (send *sr* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (unless (send (send *ri* :get-collision-status) :safe_posture) + (print "Successfully stop fail pose")) + (send *sr* :reset-pose) + (send *ri* :angle-vector (send *sr* :angle-vector) 3000) + (send *ri* :wait-interpolation) + (if (send (send *ri* :get-collision-status) :safe_posture) + (print "Successfully return to safe pose")) + t) + +(defun samplerobot-collision-detector-demo2 () + "3. CollisionCheck in fail pose with 0.1[m] tolerance" + (send *ri* :set-tolerance :tolerance 0.1) + (send *sr* :reset-pose) + (send *sr* :rarm :move-end-pos (float-vector -50 150 50) :world :rotation-axis nil) + (send *ri* :angle-vector (send *sr* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (unless (send (send *ri* :get-collision-status) :safe_posture) + (print "Successfully stop fail pose (0.1[m] tolerance)")) + (send *sr* :reset-pose) + (send *ri* :angle-vector (send *sr* :angle-vector) 3000) + (send *ri* :wait-interpolation) + (if (send (send *ri* :get-collision-status) :safe_posture) + (print "Successfully return to safe pose")) + (send *ri* :set-tolerance :tolerance 0.0) + t) + +(defun samplerobot-collision-detector-demo () + (samplerobot-collision-detector-init) + (samplerobot-collision-detector-demo0) + (samplerobot-collision-detector-demo1) + (samplerobot-collision-detector-demo2) + ) + +(warn ";; (samplerobot-collision-detector-demo)~%") \ No newline at end of file diff --git a/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-data-logger.l b/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-data-logger.l new file mode 100644 index 000000000..d9be66d8b --- /dev/null +++ b/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-data-logger.l @@ -0,0 +1,42 @@ +#!/usr/bin/env roseus + +;; $ rtmlaunch hrpsys_ros_bridge samplerobot.launch +;; $ roseus samplerobot-data-logger.l "(samplerobot-data-logger-demo)" + +(load "package://hrpsys_ros_bridge/euslisp/samplerobot-interface.l") + +(defun samplerobot-data-logger-init () + (samplerobot-init) + t) + +(defun samplerobot-data-logger-demo0 () + "1. Set max ring-buffer length : 200 [loop] * 0.002 [s] = 0.4 [s] data" + (send *ri* :set-log-maxlength 200) + (print "maxLength() =>OK") + t) + +(defun samplerobot-data-logger-demo1 () + "2. Clear buffer" + (send *ri* :start-log) + (print "clear() =>OK") + t) + +(defun samplerobot-data-logger-demo2 () + "3. Save log" + (send *sr* :reset-pose) + (send *ri* :angle-vector (send *sr* :angle-vector) 2000) + (send *ri* :wait-interpolation) + ;; Save log files for each ports as /tmp/test-samplerobot-log.* + ;; file names are /tmp/test-samplerobot-log.[RTCName]_[PortName], c.f., /tmp/test-samplerobot-log.sh_qOut ... etc + (send *ri* :save-log "/tmp/test-samplerobot-log") + (find-if #'(lambda (x) (substringp "test-samplerobot-log" x)) (directory "/tmp")) + ) + +(defun samplerobot-data-logger-demo () + (samplerobot-data-logger-init) + (samplerobot-data-logger-demo0) + (samplerobot-data-logger-demo1) + (samplerobot-data-logger-demo2) + ) + +(warn ";; (samplerobot-data-logger-demo)~%") \ No newline at end of file diff --git a/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-impedance-controller.l b/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-impedance-controller.l new file mode 100644 index 000000000..d20fac8aa --- /dev/null +++ b/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-impedance-controller.l @@ -0,0 +1,55 @@ +#!/usr/bin/env roseus + +;; $ rtmlaunch hrpsys_ros_bridge samplerobot.launch +;; $ roseus samplerobot-impedance-controller.l "(samplerobot-impedance-controller-demo)" + +(load "package://hrpsys_ros_bridge/euslisp/samplerobot-interface.l") + +(defun samplerobot-impedance-controller-init () + (samplerobot-init) + ;; set initial pose + (send *sr* :reset-pose) + (send *ri* :angle-vector (send *sr* :angle-vector) 1000) + (send *ri* :wait-interpolation) + t) + +(defun samplerobot-impedance-controller-demo0 () + "1. get parameter" + (pprint (send (send *ri* :get-impedance-controller-param :rarm) :slots)) + (print "getImpedanceControllerParam => OK") + t) + +(defun samplerobot-impedance-controller-demo1 () + "2. set and start" + (send *ri* :start-impedance :rarm :K-r 1.0 :D-r 2.0 :M-r 0.2) + (pprint (send (send *ri* :get-impedance-controller-param :rarm) :slots)) + (print "setImpedanceControllerParam => OK") + t) + +(defun samplerobot-impedance-controller-demo2 () + "3. set ref force and moment" + (send *ri* :set-ref-force (float-vector 10 10 -10) 2000 :rarm) + (send *ri* :wait-interpolation-seq) + (send *ri* :set-ref-force (float-vector 0 0 0) 2000 :rarm) + (send *ri* :wait-interpolation-seq) + (send *ri* :set-ref-moment (float-vector 0.2 0.2 -0.5) 2000 :rarm) + (send *ri* :wait-interpolation-seq) + (send *ri* :set-ref-moment (float-vector 0 0 0) 2000 :rarm) + (send *ri* :wait-interpolation-seq) + t) + +(defun samplerobot-impedance-controller-demo3 () + "4. stop" + (send *ri* :stop-impedance :rarm) + (print "deleteImpedanceController => OK") + t) + +(defun samplerobot-impedance-controller-demo () + (samplerobot-impedance-controller-init) + (samplerobot-impedance-controller-demo0) + (samplerobot-impedance-controller-demo1) + (samplerobot-impedance-controller-demo2) + (samplerobot-impedance-controller-demo3) + ) + +(warn ";; (samplerobot-impedance-controller-demo)~%") \ No newline at end of file diff --git a/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-remove-force-offset.l b/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-remove-force-offset.l new file mode 100644 index 000000000..e5b5e7af9 --- /dev/null +++ b/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-remove-force-offset.l @@ -0,0 +1,63 @@ +#!/usr/bin/env roseus + +;; $ rtmlaunch hrpsys_ros_bridge samplerobot.launch +;; $ roseus samplerobot-remove-force-offset.l "(samplerobot-remove-force-offset-demo)" + +(load "package://hrpsys_ros_bridge/euslisp/samplerobot-interface.l") + +(defun samplerobot-remove-force-offset-init () + (samplerobot-init) + ;; initial pose + (send *sr* :reset-pose) + (send *ri* :angle-vector (send *sr* :angle-vector) 1000) + (send *ri* :wait-interpolation) + t) + +(defun samplerobot-remove-force-offset-demo0 () + "1. force and moment are large because of link offsets" + ;; check force moment values + (let ((rfm (norm (send *ri* :state :off-force-vector :rarm))) + (lfm (norm (send *ri* :state :off-force-vector :larm)))) + (format t "no-offset-removed force moment (rhsensor) ~A => ~A~%" rfm (> rfm 1e-2)) + (format t "no-offset-removed force moment (lhsensor) ~A => ~A~%" lfm (> lfm 1e-2)) + (and (> rfm 1e-2) (> lfm 1e-2)) + )) + +(defun samplerobot-remove-force-offset-demo1 () + "2. Set link offsets + link_offset_centroid and link_offset_mass are identified value." + (send *ri* :set-forcemoment-offset-param :rarm :force-offset #f(0 0 0) :moment-offset #f(0 0 0) :link-offset-centroid #f(0 0.0368 -0.076271) :link-offset-mass 0.800011) + (send *ri* :set-forcemoment-offset-param :larm :force-offset #f(0 0 0) :moment-offset #f(0 0 0) :link-offset-centroid #f(0 -0.0368 -0.076271) :link-offset-mass 0.800011) + (let ((rarm-ok (= (send (send *ri* :get-forcemoment-offset-param :rarm) :link_offset_mass) 0.800011)) + (larm-ok (= (send (send *ri* :get-forcemoment-offset-param :larm) :link_offset_mass) 0.800011))) + (if rarm-ok (print "getForceMomentOffsetParam(\"rhsensor\") => OK")) + (if larm-ok (print "getForceMomentOffsetParam(\"lhsensor\") => OK")) + (and rarm-ok larm-ok))) + +(defun samplerobot-remove-force-offset-demo2 () + "3. force and moment are reduced" + ;; wait for 200ms instead of sleep + (send *ri* :angle-vector (send *sr* :angle-vector) 200) + (send *ri* :wait-interpolation) + ;; check force moment values + (let ((rfm (norm (send *ri* :state :off-force-vector :rarm))) + (lfm (norm (send *ri* :state :off-force-vector :larm)))) + (format t "offset-removed force moment (rhsensor) ~A => ~A~%" rfm (< rfm 1e-2)) + (format t "offset-removed force moment (lhsensor) ~A => ~A~%" lfm (< lfm 1e-2)) + (and (< rfm 1e-2) (< lfm 1e-2)))) + +(defun samplerobot-remove-force-offset-demo3 () + "4. dump and load parameter file" + (send *ri* :dump-forcemoment-offset-params "/tmp/test-rmfo" :set-robot-date-string nil) + (send *ri* :load-forcemoment-offset-params "/tmp/test-rmfo") + ) + +(defun samplerobot-remove-force-offset-demo () + (samplerobot-remove-force-offset-init) + (samplerobot-remove-force-offset-demo0) + (samplerobot-remove-force-offset-demo1) + (samplerobot-remove-force-offset-demo2) + (samplerobot-remove-force-offset-demo3) + ) + +(warn ";; (samplerobot-remove-force-offset-demo)~%") diff --git a/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-sequence-player.l b/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-sequence-player.l new file mode 100644 index 000000000..3875f295e --- /dev/null +++ b/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-sequence-player.l @@ -0,0 +1,37 @@ +#!/usr/bin/env roseus + +;; $ rtmlaunch hrpsys_ros_bridge samplerobot.launch +;; $ roseus samplerobot-sequence-player.l "(samplerobot-sequence-player-demo)" + +(load "package://hrpsys_ros_bridge/euslisp/samplerobot-interface.l") + +(defun samplerobot-sequence-player-init () + (samplerobot-init) + t) + +(defun samplerobot-sequence-player-demo0 () + "1. :angle-vector (setJointAngles) and :wait-interpolation (waitInterpolation)" + (send *ri* :angle-vector (send *sr* :angle-vector) 700) + (send *ri* :wait-interpolation) + (eps= (distance (send *ri* :state :potentio-vector) (send *sr* :angle-vector)) 0.0) + ) + +(defun samplerobot-sequence-player-demo1 () + "2. :set-interpolation-mode" + (send *ri* :set-interpolation-mode :linear) + (send *sr* :reset-manip-pose) + (send *ri* :angle-vector (send *sr* :angle-vector) 500) + (send *ri* :wait-interpolation) + (send *ri* :set-interpolation-mode :hoffarbib) + (send *sr* :reset-pose) + (send *ri* :angle-vector (send *sr* :angle-vector) 500) + (send *ri* :wait-interpolation) + t) + +(defun samplerobot-sequence-player-demo () + (samplerobot-sequence-player-init) + (samplerobot-sequence-player-demo0) + (samplerobot-sequence-player-demo1) + ) + +(warn ";; (samplerobot-sequence-player-demo)~%") \ No newline at end of file diff --git a/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-stabilizer.l b/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-stabilizer.l new file mode 100644 index 000000000..d4dabce64 --- /dev/null +++ b/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-stabilizer.l @@ -0,0 +1,51 @@ +#!/usr/bin/env roseus + +;; $ rtmlaunch hrpsys_ros_bridge samplerobot.launch +;; $ roseus samplerobot-stabilizer.l "(samplerobot-stabilizer-demo)" + +(load "package://hrpsys_ros_bridge/euslisp/samplerobot-interface.l") + +(defun samplerobot-stabilizer-init () + (samplerobot-init) + (send *sr* :reset-pose) + (send *ri* :angle-vector (send *sr* :angle-vector) 1000) + (send *ri* :wait-interpolation) + t) + +(defun samplerobot-stabilizer-demo0 () + "1. getParameter" + (pprint (send (send *ri* :get-st-param) :slots)) + (print "getParameter() => OK") + t) + +(defun samplerobot-stabilizer-demo1 () + "2. setParameter" + (let ((k-tpcc-p (float-vector 0.2 0.2)) + (k-tpcc-x (float-vector 4.0 4.0)) + (k-brot-p (float-vector 0.0 0.0))) + (send *ri* :set-st-param + :k-tpcc-p k-tpcc-p + :k-tpcc-x k-tpcc-x + :k-brot-p k-brot-p) + (if (and (eps-v= (send (send *ri* :get-st-param) :k_tpcc_p) k-tpcc-p) + (eps-v= (send (send *ri* :get-st-param) :k_tpcc_x) k-tpcc-x) + (eps-v= (send (send *ri* :get-st-param) :k_brot_p) k-brot-p)) + (print "setParameter() => OK"))) + t) + +(defun samplerobot-stabilizer-demo2 () + "3. start and stop st" + (send *ri* :start-st) + (send *ri* :go-pos 0.5 0.1 10) + (send *ri* :stop-st) + (print "Start and Stop Stabilizer => OK") + t) + +(defun samplerobot-stabilizer-demo () + (samplerobot-stabilizer-init) + (samplerobot-stabilizer-demo0) + (samplerobot-stabilizer-demo1) + (samplerobot-stabilizer-demo2) + ) + +(warn ";; (samplerobot-stabilizer-demo)~%") \ No newline at end of file diff --git a/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-terrain-walk.l b/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-terrain-walk.l new file mode 100644 index 000000000..7d7338399 --- /dev/null +++ b/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-terrain-walk.l @@ -0,0 +1,100 @@ +#!/usr/bin/env roseus + +;; for SlopeUpDown +;; $ rtmlaunch hrpsys_ros_bridge samplerobot.launch PROJECT_FILE:=`rospack find hrpsys`/share/hrpsys/samples/SampleRobot/SampleRobot.TerrainFloor.SlopeUpDown.xml +;; $ roseus samplerobot-terrain-walk.l "(demo-slope-updown)" +;; for StairUp +;; $ rtmlaunch hrpsys_ros_bridge samplerobot.launch PROJECT_FILE:=`rospack find hrpsys`/share/hrpsys/samples/SampleRobot/SampleRobot.TerrainFloor.StairUp.xml +;; $ roseus samplerobot-terrain-walk.l "(demo-stair-up)" +;; for StairDown +;; $ rtmlaunch hrpsys_ros_bridge samplerobot.launch PROJECT_FILE:=`rospack find hrpsys`/share/hrpsys/samples/SampleRobot/SampleRobot.TerrainFloor.StairDown.xml +;; $ roseus samplerobot-terrain-walk.l "(demo-stair-down)" + + +(load "package://hrpsys_ros_bridge/euslisp/samplerobot-interface.l") + +(defun init () + (samplerobot-init) + ) + +(defun init-pose () + (send *sr* :reset-pose) + (send *sr* :legs :move-end-pos #f(0 0 70)) + (send *ri* :angle-vector (send *sr* :angle-vector) 1000) + (send *ri* :wait-interpolation) + ) +(init) +(init-pose) + +(defun setup-gait-generator-param (&optional (use-rectangle t)) + (send* *ri* :set-gait-generator-param + :default-double-support-ratio 0.3 + :default-step-time 1.2 + :swing-trajectory-delay-time-offset 0.2 + (if use-rectangle + (list :default-orbit-type :RECTANGLE) + (list + :stair-trajectory-way-point-offset #f(0.04 0 0) + :default-orbit-type :STAIR))) + ) + +(defun stair-walk (&optional (stair-height (* 1e3 0.1524))) + (let ((stair-stride-x (* 1e3 0.25)) + (floor-stride-x (* 1e3 0.157)) + (init-step-x 0) + (init-step-z 0) + (fs)) + (dolist (step-idx (list 1 2 3 4)) + (setup-gait-generator-param (= (mod step-idx 2) 1)) + (setq fs (list (make-coords :pos (float-vector init-step-x -90 init-step-z) :name :rleg) + (make-coords :pos (float-vector (+ init-step-x stair-stride-x) 90 (+ init-step-z stair-height)) :name :lleg))) + (send *ri* :set-foot-steps fs) + (send *ri* :wait-foot-steps) + (setq fs (list (make-coords :pos (float-vector (+ init-step-x stair-stride-x) 90 (+ init-step-z stair-height)) :name :lleg) + (make-coords :pos (float-vector (+ init-step-x stair-stride-x) -90 (+ init-step-z stair-height)) :name :rleg))) + (send *ri* :set-foot-steps fs) + (send *ri* :wait-foot-steps) + (setq fs (list (make-coords :pos (float-vector (+ init-step-x stair-stride-x) 90 (+ init-step-z stair-height)) :name :lleg) + (make-coords :pos (float-vector (+ init-step-x stair-stride-x floor-stride-x) -90 (+ init-step-z stair-height)) :name :rleg) + (make-coords :pos (float-vector (+ init-step-x stair-stride-x floor-stride-x) 90 (+ init-step-z stair-height)) :name :lleg))) + (send *ri* :set-foot-steps fs) + (send *ri* :wait-foot-steps) + ) + )) + +(defun demo-slope-updown () + (print "Start stlop up down") + (let ((fs-list (list (make-coords :pos (scale 1e3 #f(0.8 -0.09 0.0)) :rot (quaternion2matrix (float-vector 1.0 0.0 2.775558e-17 0.0)) :name :rleg) + (make-coords :pos (scale 1e3 #f(1.0953 0.09 0.030712)) :rot (quaternion2matrix (float-vector 0.991445 0.0 -0.130526 0.0)) :name :lleg) + (make-coords :pos (scale 1e3 #f(1.28848 -0.09 0.082475)) :rot (quaternion2matrix (float-vector 0.991445 0.0 -0.130526 0.0)) :name :rleg) + (make-coords :pos (scale 1e3 #f(1.38508 0.09 0.108357)) :rot (quaternion2matrix (float-vector 0.991445 0.0 -0.130526 0.0)) :name :lleg) + (make-coords :pos (scale 1e3 #f(1.38508 -0.09 0.108357)) :rot (quaternion2matrix (float-vector 0.991445 0.0 -0.130526 0.0)) :name :rleg) + (make-coords :pos (scale 1e3 #f(1.54959 0.09 0.125863)) :rot (quaternion2matrix (float-vector 0.991445 0.0 0.130526 0.0)) :name :lleg) + (make-coords :pos (scale 1e3 #f(1.74277 -0.09 0.074099)) :rot (quaternion2matrix (float-vector 0.991445 0.0 0.130526 0.0)) :name :rleg) + (make-coords :pos (scale 1e3 #f(1.79107 0.09 0.061158)) :rot (quaternion2matrix (float-vector 0.991445 0.0 0.130526 0.0)) :name :lleg) + (make-coords :pos (scale 1e3 #f(2.05 -0.09 0.0)) :rot (quaternion2matrix (float-vector 1.0 0.0 0.0 0.0)) :name :rleg) + (make-coords :pos (scale 1e3 #f(2.05 0.09 0.0)) :rot (quaternion2matrix (float-vector 1.0 0.0 0.0 0.0)) :name :lleg)))) + (setup-gait-generator-param) + (send *ri* :start-auto-balancer) + (dotimes (fs-idx (- (length fs-list) 1)) + (send *ri* :set-foot-steps (list (elt fs-list fs-idx) (elt fs-list (+ 1 fs-idx)))) + (send *ri* :wait-foot-steps) + ) + (send *ri* :stop-auto-balancer) + )) + +(defun demo-Stair-Up () + (print "Start stair up") + (setup-gait-generator-param) + (send *ri* :start-auto-balancer) + (stair-walk) + (send *ri* :stop-auto-balancer) + ) + +(defun demo-Stair-Down () + (print "Start stair down") + (setup-gait-generator-param) + (send *ri* :start-auto-balancer) + (stair-walk (* 1e3 -0.1524)) + (send *ri* :stop-auto-balancer) + ) diff --git a/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-unittest.l b/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-unittest.l new file mode 100644 index 000000000..17ca661dd --- /dev/null +++ b/hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-unittest.l @@ -0,0 +1,32 @@ +#!/usr/bin/env roseus + +(require :unittest "lib/llib/unittest.l") +(init-unit-test) + +;; Wait until Ros bridges are prepared. +(ros::roseus "unittest") +(unless (boundp '*tfl*) + (defvar *tfl* (instance ros::transform-listener :init))) +(send *tfl* :wait-for-transform "WAIST_LINK0" "odom" (ros::time) 20.0) +;; Load demo file which is specified next to this file in argument +(let ((name (elt lisp::*eustop-argument* (1+ (position-if #'(lambda (x) (substringp "samplerobot-unittest.l" x)) lisp::*eustop-argument*))))) + (defvar *test-prefix* (pathname-name name)) + (load name)) + +;; Test init +(let ((func (find-if #'(lambda (x) ;; find init function + (substringp (format nil "~A-init" *test-prefix*) (string-downcase x))) + (functions)))) + (eval `(deftest ,(read-from-string (format nil "test-~A" func)) + (assert (,func))))) + +;; Test demos +(dolist (funcs (remove-if-not #'(lambda (x) ;; find demo functions + (and (substringp (format nil "~A-demo" *test-prefix*) (string-downcase x)) + (not (string= (format nil "~A-demo" *test-prefix*) (string-downcase x))))) + (functions))) + (eval `(deftest ,(read-from-string (format nil "test-~A" funcs)) + (assert (,funcs))))) + +(run-all-tests) +(exit 0) \ No newline at end of file diff --git a/hrpsys_ros_bridge/test/hrpsys-samples/test_samplerobot_euslisp_unittests.launch b/hrpsys_ros_bridge/test/hrpsys-samples/test_samplerobot_euslisp_unittests.launch new file mode 100644 index 000000000..4e8d1d07b --- /dev/null +++ b/hrpsys_ros_bridge/test/hrpsys-samples/test_samplerobot_euslisp_unittests.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + From 14dd212ff9dd40094cbeabaa9c1e2da6511586ac Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Wed, 26 Aug 2015 16:43:41 +0900 Subject: [PATCH 2/4] [hrpsys_ros_bridge/.gitignore] Do not ignore models because this is tracked by git --- hrpsys_ros_bridge/.gitignore | 1 - 1 file changed, 1 deletion(-) diff --git a/hrpsys_ros_bridge/.gitignore b/hrpsys_ros_bridge/.gitignore index ca0664c29..a61a18c3b 100644 --- a/hrpsys_ros_bridge/.gitignore +++ b/hrpsys_ros_bridge/.gitignore @@ -1,5 +1,4 @@ idl/ -models/ msg/Img_CameraCaptureService.msg msg/OpenHRP_* msg/OpenRTM_* From 08a53ba6aa45dd1dad56798ebf82341928fedce9 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Wed, 26 Aug 2015 16:44:11 +0900 Subject: [PATCH 3/4] [hrpsys_ros_bridge/catkin.cmake] Add samplerobot euslisp test as .launch file instead of .test file --- hrpsys_ros_bridge/catkin.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/hrpsys_ros_bridge/catkin.cmake b/hrpsys_ros_bridge/catkin.cmake index 3b27fb215..a84d4e3af 100644 --- a/hrpsys_ros_bridge/catkin.cmake +++ b/hrpsys_ros_bridge/catkin.cmake @@ -202,4 +202,5 @@ add_rostest(test/test-import-python.test) find_package(catkin COMPONENTS roseus QUIET) if(roseus_FOUND) generate_eusdoc(euslisp/rtm-ros-robot-interface.l) + add_rostest(test/hrpsys-samples/test_samplerobot_euslisp_unittests.launch) endif() From e0ba7e758a55dcbe3dcd781eb22a223b24356a9a Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Wed, 26 Aug 2015 16:45:25 +0900 Subject: [PATCH 4/4] [.travis.yml] Add travis test for euslisp test. --- .travis.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.travis.yml b/.travis.yml index 3af267651..f4d4e0489 100644 --- a/.travis.yml +++ b/.travis.yml @@ -20,6 +20,7 @@ env: matrix: - ROS_DISTRO=hydro USE_DEB=true - ROS_DISTRO=hydro USE_DEB=false NOT_TEST_INSTALL=true + - ROS_DISTRO=hydro USE_DEB=false NOT_TEST_INSTALL=true EXTRA_DEB="ros-hydro-roseus ros-hydro-euscollada ros-hydro-pr2eus" CATKIN_PARALLEL_JOBS='-p1' - ROS_DISTRO=hydro USE_DEB=true NOT_TEST_INSTALL=true INSTALL_SRC="http://github.com/start-jsk/rtmros_tutorials" TEST_PKGS="hrpsys_ros_bridge_tutorials" INSTALL_SRC_SECURE="git@github.com:start-jsk/rtmros_hrp2" TEST_PKGS_SECURE="jsk_hrp2_ros_bridge" - ROS_DISTRO=hydro USE_DEB=source NOT_TEST_INSTALL=true - ROS_DISTRO=indigo USE_DEB=true