gazebo hrp2jsk grasp

README.txt

$ 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

hrp2jsk-reach.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)
  )

(progn
  (send *robot* :reset-manip-pose)
  (send *robot* :larm :thumb-r :joint-angle -100)
  (send *robot* :head :neck-p :joint-angle 43)
  (send *ri* :angle-vector (send *robot* :angle-vector) 1000)
  (send *ri* :wait-interpolation)

  (send *robot* :larm :move-end-pos #f(200 -200 -80) :world :rotation-axis :z)
  (send *robot* :legs :move-end-pos #f(0 0 145) :world)
  (send *ri* :angle-vector (send *robot* :angle-vector) 1000)
  (send *ri* :wait-interpolation)

  (send *robot* :larm :move-end-pos #f(70 -90 0) :world :rotation-axis :z)
  (send *ri* :angle-vector (send *robot* :angle-vector) 1000)
  (send *ri* :wait-interpolation)

  (send *robot* :larm :thumb-r :joint-angle 13)
  (send *ri* :angle-vector (send *robot* :angle-vector) 5000)
  (send *ri* :wait-interpolation)
  )

(progn
  (send *robot* :init-pose)
  (send *ri* :angle-vector (send *robot* :angle-vector) 1000)
  )

table_top.world

<?xml version="1.0" ?>
<sdf version="1.4">
  <world name="default">
    <physics type="ode">
      <gravity>0 0 -9.81</gravity>
      <ode>
        <solver>
          <type>quick</type>
          <iters>50</iters>
          <sor>1.4</sor>
        </solver>
        <constraints>
          <cfm>0.0</cfm>
          <erp>0.2</erp>
          <contact_max_correcting_vel>100.0</contact_max_correcting_vel>
          <contact_surface_layer>0.0</contact_surface_layer>
        </constraints>
      </ode>
      <real_time_update_rate>1000</real_time_update_rate>
      <max_step_size>0.001</max_step_size>
      <max_contacts>40</max_contacts>
    </physics>
    <!-- A global light source -->
    <include>
      <uri>model://sun</uri>
    </include>
    <!-- A ground plane -->
    <include>
      <uri>model://ground_plane</uri>
    </include>
    <!-- Objects -->
    <include>
      <uri>model://room73b2-unknown-1200-desk-0-0_static</uri>
      <pose>1.0 0.0 0.0 0.0 0.0 1.5708</pose>
    </include>
    <include>
      <uri>model://room73b2-cup-0</uri>
      <pose>0.5 0.0 0.7 0.0 0.0 1.5708</pose>
    </include>
  </world>
</sdf>