mmurooka

(load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-interface.l")
(load "package://hrpsys_ros_bridge/euslisp/datalogger-log-parser.l")

(defun init
  ()
  (hrp2jsknt-init)
  (setq *robot* *hrp2jsknt*)
  (objects (list *robot*))
  )

mmurooka

(load "package://pr2eus/pr2.l")
(setq *robot* (pr2))
(objects (list *robot*))

(send *robot* :reset-pose)
(send *robot* :inverse-kinematics
      (list (make-coords :pos (float-vector 370 -200 770))
            (make-coords :pos (float-vector 370 200 790)))
      :move-target (list (send *robot* :larm :end-coords) (send *robot* :rarm :end-coords))
      :list-list(list (send *robot* :link-list (send (send *robot* :larm :end-coords) :parent)) (send *robot* :link-list (send (send *robot* :rarm :end-coords) :parent)))

mmurooka

<launch>
  <!-- checker board detector -->
  <!-- <param name="single" value="1" /> -->
  <param name="display" type="int" value="0" />
  <param name="rect0_size_x" value="0.03" />
  <param name="rect0_size_y" value="0.03" />
  <param name="grid0_size_x" value="6" />
  <param name="grid0_size_y" value="5" />
  <param name="type0" value="test_object" />

mmurooka

<launch>
  <arg name="INPUT" default="/camera/depth/points" />
  <arg name="SELF_FILTER_PARAM" default="robot_self_filter.yaml" />

  <node pkg="robot_self_filter"
        type="self_filter" clear_params="true"
        name="self_filter" respawn="true" output="screen">
      <remap from="cloud_in" to="$(arg INPUT)" />
      <remap from="cloud_out" to="~output" />
      <rosparam command="load" file="$(arg SELF_FILTER_PARAM)" />

mmurooka

$ roslaunch hrpsys_gazebo_tutorials gazebo_hrp2jsk_no_controllers.launch WORLD:=`rospack find hrpsys_gazebo_tutorials`/worlds/table_top.world
$ rtmlaunch hrpsys_gazebo_tutorials hrp2jsk_hrpsys_bringup.launch

mmurooka

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

mmurooka

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

(defun setup-real
  ()
  (jaxon-init)
  (setq *robot* *jaxon*)
  (objects (list *robot*))
  (send *ri* :start-grasp)
  )

mmurooka

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

(defun setup
  ()
  (hrp2jsknts-init)
  (send *ri* :start-auto-balancer)
  (setq *robot* *hrp2jsknts*)
  (objects (list *robot*))
  )

mmurooka

(defun make-polyhedron (top middles bottom)
  (let (upper-faces lower-faces)
    (setq middles (mapcar #'list middles))
    (make-face-from-vertices middles)
    (setq upper-faces (geometry::make-conic-side-faces (list top) middles :upper)
          lower-faces (geometry::make-conic-side-faces (list bottom) (reverse middles) :lower))
    (send (instantiate *body-class*) :init
          :faces (flatten (append upper-faces lower-faces))
          :approximated t
          :primitive (list ':polyhedron (append (list top) (mapcar #'car middles) (list bottom)))

mmurooka

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

(hrp2jsknt-init)
(setq *robot* *hrp2jsknt*)

(send *robot* :angle-vector #f(-4.40826 0.0 -15.5398 37.2128 -19.2137 0.0 0.0 -4.40717 0.0 -16.5779 37.4299 -18.3927 0.0 0.0 12.4367 -0.346437 0.0 0.0 -62.8846 8.83576 59.0133 -74.8765 -36.3933 -86.4512 19.8172 15.0 -45.5764 8.92094 -28.0361 -52.815 -24.1275 -4.62376 2.76207 -15.0))
(send *ri* :angle-vector (send *robot* :angle-vector) 1000)

(send *ri* :start-impedance :rarm)
(send *ri* :set-ref-force #f(0 10 0) 2000 :rarm)