Add tutorial for manipulation framework

CMakeLists.txt

@@ -61,29 +61,46 @@ INSTALL (TARGETS hpp-tutorial-server DESTINATION ${CMAKE_INSTALL_BINDIR})
 SET(CATKIN_PACKAGE_SHARE_DESTINATION
   ${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME})
 
-install(FILES launch/tutorial.launch
+install(FILES
+  launch/tutorial.launch
+  launch/tutorial_manipulation.launch
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
   )
-install(FILES urdf/pr2.urdf
+install(FILES
+  urdf/pr2.urdf
+  urdf/box.urdf
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
   )
-install(FILES srdf/pr2.srdf 
+install(FILES
+  srdf/pr2.srdf
+  srdf/pr2_manipulation.srdf
+  srdf/box.srdf
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
   )
-install(FILES rviz/config.rviz
+install(FILES
+  rviz/config.rviz
+  rviz/config_manipulation.rviz
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz
   )
 install (FILES
   src/hpp/corbaserver/pr2/robot.py
   src/hpp/corbaserver/pr2/__init__.py
   DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/pr2)
+install (FILES
+  src/hpp/corbaserver/manipulation/pr2/robot.py
+  src/hpp/corbaserver/manipulation/pr2/__init__.py
+  DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/manipulation/pr2)
 
 # Installation for documentation
-install(FILES launch/tutorial.launch
+install(FILES
+  launch/tutorial.launch
+  launch/tutorial_manipulation.launch
   DESTINATION
   ${CMAKE_INSTALL_DATAROOTDIR}/doc/${PROJECT_NAME}/doxygen-html/launch
   )
-install(FILES urdf/pr2.urdf
+install(FILES
+  urdf/pr2.urdf
+  urdf/box.urdf
   DESTINATION
   ${CMAKE_INSTALL_DATAROOTDIR}/doc/${PROJECT_NAME}/doxygen-html/urdf
   )

launch/tutorial_manipulation.launch

+<launch>
+  <param name="kitchen_description"
+    textfile="$(find iai_maps)/urdf/kitchen_area.urdf"/>
+  <param name="robot_description"
+    textfile="$(find hpp_tutorial)/urdf/pr2.urdf"/>
+  <param name="box_description"
+    textfile="$(find hpp_tutorial)/urdf/box.urdf"/>
+  <node
+    pkg="tf" type="static_transform_publisher" name="odom_broadcaster"
+    args="0 0 0 0 0 0 1 /map /odom 1000" />
+  <node name="robot_state_publisher"
+    pkg="robot_state_publisher"
+    type="state_publisher"
+    respawn="true">
+    <param name="tf_prefix" value="pr2" />
+    <remap from="joint_states" to="pr2/joint_states"/>
+  </node>
+
+  <node pkg="tf" type="static_transform_publisher"
+    name="kitchen_link_broadcaster"
+    args="0 0 0 0 0 0 /map /iai_kitchen/kitchen_link 200">
+  </node>
+  <node pkg="robot_state_chain_publisher" type="state_chain_publisher"
+    name="kitchen_state_publisher" output="screen">
+    <param name="tf_prefix" value="/iai_kitchen"/>
+    <param name="publish_frequency" value="2"/>
+    <remap from="robot_description" to="kitchen_description" />
+  </node>
+  <node name="rviz" pkg="rviz" type="rviz" respawn="false"
+    args="-d $(find hpp_tutorial)/rviz/config_manipulation.rviz">
+  </node>
+</launch>

script/tutorial_manipulation.py

+from hpp.corbaserver.manipulation.pr2 import Robot
+
+robot = Robot ('pr2')
+robot.client.basic.problem.selectPathPlanner ("M-RRT")
+robot.loadObjectModel ('box', 'freeflyer', 'hpp_tutorial', 'box', '', '')
+robot.buildCompositeRobot ('pr2-box', ['pr2', 'box'])
+robot.client.basic.obstacle.loadObstacleModel ("iai_maps", "kitchen_area", "")
+
+robot.setJointBounds ("pr2/base_joint_xy" , [-5,-2,-5.2,-2.7]     )
+robot.setJointBounds ("box/base_joint_xyz", [-5.1,-2,-5.2,-2.7,0,1.5])
+
+from hpp_ros.manipulation import ScenePublisher
+r = ScenePublisher (robot)
+
+q_init = robot.getCurrentConfig ()
+rank = robot.rankInConfiguration ['pr2/l_gripper_l_finger_joint']
+q_init [rank] = 0.5
+rank = robot.rankInConfiguration ['pr2/l_gripper_r_finger_joint']
+q_init [rank] = 0.5
+q_goal = q_init [::]
+q_init [0:2] = [-3.2, -4]
+rank = robot.rankInConfiguration ['pr2/torso_lift_joint']
+q_init [rank] = 0.2
+rank = robot.rankInConfiguration ['box/base_joint_xyz']
+q_init [rank:rank+3] = [-2.5, -4, 0.8]
+r (q_init)
+
+q_goal [0:2] = [-3.2, -4]
+rank = robot.rankInConfiguration ['pr2/l_shoulder_lift_joint']
+q_goal [rank] = 0.5
+rank = robot.rankInConfiguration ['pr2/l_elbow_flex_joint']
+q_goal [rank] = -0.5
+rank = robot.rankInConfiguration ['pr2/r_shoulder_lift_joint']
+q_goal [rank] = 0.5
+rank = robot.rankInConfiguration ['pr2/r_elbow_flex_joint']
+q_goal [rank] = -0.5
+rank = robot.rankInConfiguration ['box/base_joint_xyz']
+q_goal [rank:rank+3] = [-4.8, -4.6, 0.9]
+rank = robot.rankInConfiguration ['box/base_joint_SO3']
+q_goal [rank:rank+4] = [0, 0, 0, 1]
+r (q_goal)
+
+robot.client.basic.problem.resetRoadmap ()
+robot.client.basic.problem.selectPathOptimizer ('None')
+robot.client.basic.problem.setErrorThreshold (1e-3)
+robot.client.basic.problem.setMaxIterations (40)
+
+from hpp.corbaserver.manipulation import ProblemSolver
+p = ProblemSolver (robot)
+
+p.createGrasp ('l_grasp', 'pr2/l_gripper', 'box/handle')
+p.createGrasp ('l_grasp_passive', 'pr2/l_gripper', 'box/handle')
+
+p.createPreGrasp ('l_pregrasp', 'pr2/l_gripper', 'box/handle')
+
+rankcfg = 0; rankvel = 0; lockbox = list ();
+for axis in ['x','y','z']:
+  p.createLockedDofConstraint ('box_lock_'  + axis, 'box/base_joint_xyz', 0, rankcfg, rankvel)
+  p.createLockedDofConstraint ('box_lock_r' + axis, 'box/base_joint_SO3', 0, rankcfg + 1, rankvel)
+  p.isLockedDofParametric ('box_lock_'  + axis ,True)
+  p.isLockedDofParametric ('box_lock_r' + axis ,True)
+  lockbox.append ('box_lock_'  + axis)
+  lockbox.append ('box_lock_r' + axis)
+  rankcfg = rankcfg + 1
+  rankvel = rankvel + 1
+
+locklhand = ['l_l_finger','l_r_finger'];
+p.createLockedDofConstraint ('l_l_finger' , 'pr2/l_gripper_l_finger_joint', 0.5, 0, 0)
+p.createLockedDofConstraint ('l_r_finger', 'pr2/l_gripper_r_finger_joint', 0.5, 0, 0)
+
+jointNames = dict ()
+jointNames['all'] = robot.getJointNames ()
+jointNames['pr2'] = list ()
+jointNames['allButPR2LeftArm'] = list ()
+for n in jointNames['all']:
+  if n.startswith ("pr2"):
+    jointNames['pr2'].append (n)
+  if not n.startswith ("pr2/l_gripper"):
+    jointNames['allButPR2LeftArm'].append (n)
+robot.client.basic.problem.setPassiveDofs ('l_grasp_passive', jointNames['pr2'])
+
+graph = robot.client.manipulation.graph
+id = dict()
+id["graph"   ] = graph.createGraph ('pr2-box')
+id["subgraph"] = graph.createSubGraph ('lefthand')
+
+id["box" ] = graph.createNode (id["subgraph"], 'box')
+id["free"] = graph.createNode (id["subgraph"], 'free')
+
+id["ungrasp"] = graph.createWaypointEdge (id["box"], id["free"], "ungrasp", 1, False)
+id["ungrasp_w"], id["ungrasp_w_node"] = graph.getWaypoint (id["ungrasp"])
+
+id[  "grasp"] = graph.createWaypointEdge (id["free"], id["box"],   "grasp", 10, True)
+id["grasp_w"], id["grasp_w_node"] = graph.getWaypoint (id["grasp"])
+
+id["move_free" ] = graph.createEdge (id["free"    ], id["free"    ], "move_free" , 1 , False)
+
+id["keep_grasp"] = graph.createEdge (id["box"], id["box"], "keep_grasp", 5, False)
+id["keep_grasp_ls"] = graph.createLevelSetEdge (id["box"], id["box"], "keep_grasp_ls", 10, False)
+
+graph.setNumericalConstraints (id["box"], ['l_grasp'])
+graph.setNumericalConstraintsForPath (id["box"], ['l_grasp_passive'])
+graph.setLockedDofConstraints (id["move_free"], lockbox)
+graph.setLockedDofConstraints (id["ungrasp"], lockbox)
+graph.setNumericalConstraints (id["ungrasp_w_node"], ['l_pregrasp', 'l_pregrasp/ineq_0', 'l_pregrasp/ineq_0.1'])
+graph.setLockedDofConstraints (id["ungrasp_w"], lockbox)
+graph.setLockedDofConstraints (id["grasp"], lockbox)
+graph.setNumericalConstraints (id["grasp_w_node"], ['l_pregrasp', 'l_pregrasp/ineq_0', 'l_pregrasp/ineq_0.1'])
+graph.setLockedDofConstraints (id["grasp_w"], lockbox)
+graph.setLevelSetConstraints  (id["keep_grasp_ls"], [], lockbox)
+graph.setLockedDofConstraints (id["graph"], locklhand)
+
+p.setInitialConfig (q_init)
+p.addGoalConfig (q_goal)

src/hpp/corbaserver/manipulation/pr2/__init__.py

+#!/usr/bin/env python
+# Copyright (c) 2014 CNRS
+# Author: Florent Lamiraux
+#
+# This file is part of hpp-corbaserver.
+# hpp-corbaserver is free software: you can redistribute it
+# and/or modify it under the terms of the GNU Lesser General Public
+# License as published by the Free Software Foundation, either version
+# 3 of the License, or (at your option) any later version.
+#
+# hpp-corbaserver is distributed in the hope that it will be
+# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# hpp-corbaserver.  If not, see
+# <http://www.gnu.org/licenses/>.
+
+from robot import Robot

src/hpp/corbaserver/manipulation/pr2/robot.py

+#!/usr/bin/env python
+# Copyright (c) 2014 CNRS
+# Author: Florent Lamiraux
+#
+# This file is part of hpp-corbaserver.
+# hpp-corbaserver is free software: you can redistribute it
+# and/or modify it under the terms of the GNU Lesser General Public
+# License as published by the Free Software Foundation, either version
+# 3 of the License, or (at your option) any later version.
+#
+# hpp-corbaserver is distributed in the hope that it will be
+# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# hpp-corbaserver.  If not, see
+# <http://www.gnu.org/licenses/>.
+
+from hpp.corbaserver.manipulation.robot import Robot as Parent
+
+##
+#  Control of robot PR2 in hpp
+#
+#  This class implements a client to the corba server implemented in
+#  hpp-corbaserver. It derive from class hpp.corbaserver.robot.Robot.
+#
+#  This class is also used to initialize a client to rviz in order to
+#  display configurations of the PR2 robot.
+#
+#  At creation of an instance, the urdf and srdf files are loaded using
+#  idl interface hpp::corbaserver::Robot::loadRobotModel.
+class Robot (Parent):
+    ##
+    #  Information to retrieve urdf and srdf files.
+    packageName = "hpp_tutorial"
+    ##
+    #  Information to retrieve urdf and srdf files.
+    urdfName = "pr2"
+    urdfSuffix = ""
+    srdfSuffix = "_manipulation"
+
+    def __init__ (self, robotName, load = True):
+        Parent.__init__ (self, robotName, "planar")
+        self.tf_root = "base_footprint"

srdf/box.srdf

+<?xml version="1.0"?>
+<robot name="box">
+  <handle name="handle">
+    <local_position> 0 0 0 1 0 0 0 </local_position>
+    <link name="base_link"/>
+  </handle>
+</robot>

urdf/box.urdf

+<robot name="box">
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <box size="0.05 0.05 0.05"/>
+      </geometry>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <box size="0.05 0.05 0.05"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>