gazebo hrp2jsk cupboard

README.txt

roslaunch hrpsys_gazebo_tutorials gazebo_hrp2jsk_no_controllers_room73b2.launch
rosrun image_view image_view image:=/xtion/rgb/image_raw # camera image
rtmlaunch hrpsys_gazebo_tutorials hrp2jsk_hrpsys_bringup.launch KINEMATICS_MODE:=true
rqt_plot /cupboard/GetVelPlugin/RelVel/twist/angular/y # plot cupboard angle

hrp2jsk-cupboard.l

(load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsk-interface.l")

(progn
  (hrp2jsk-init)
  (setq *robot* *hrp2jsk*)
  (send *robot* :reset-pose)
  (send *ri* :angle-vector (send *robot* :angle-vector) 1000)
  (send *ri* :wait-interpolation)
  (send *ri* :start-auto-balancer)
  (send *ri* :go-pos 1 0 0)
  )

(progn
  (send *robot* :reset-manip-pose)
  (send *ri* :angle-vector (send *robot* :angle-vector) 1000)
  (send *ri* :wait-interpolation)

  (send *ri* :go-pos 0.5 0 0)

  (send *ri* :start-impedance :arms)

  (dotimes (i 10)
    (send *robot* :arms :move-end-pos #f(25 0 0) :world)
    (send *ri* :angle-vector (send *robot* :angle-vector) 250)
    (send *ri* :wait-interpolation)
    )

  (dotimes (i 10)
    (send *robot* :arms :move-end-pos #f(-25 0 0) :world)
    (send *ri* :angle-vector (send *robot* :angle-vector) 250)
    (send *ri* :wait-interpolation)
    )

  (send *ri* :go-pos -0.5 0 0)
  )