expand functionality of MoveGroupInterface

demos/plan.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python
+
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2013, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Author: Ioan Sucan
+
+import sys
+import rospy
+from moveit_commander import RobotCommander, roscpp_initialize, roscpp_shutdown
+from moveit_msgs.msg import RobotState
+
+if __name__=='__main__':
+
+    roscpp_initialize(sys.argv)
+    rospy.init_node('moveit_py_demo', anonymous=True)
+
+    robot = RobotCommander()
+    rospy.sleep(1)
+
+    print "Current state:"
+    print robot.get_current_state()
+
+    # plan to a random location 
+    a = robot.right_arm
+    a.set_start_state(RobotState())
+    r = a.get_random_joint_values()
+    print "Planning to random joint position: "
+    print r
+    p = a.plan(r)
+    print "Solution:"
+    print p
+
+    roscpp_shutdown()

src/moveit_commander/move_group.py

@@ -119,6 +119,12 @@ def get_random_pose(self, end_effector_link = ""):
         else:
             raise MoveItCommanderException("There is no end effector to get the pose of")
 
+    def set_start_state_to_current_state(self):
+        self._g.set_start_state_to_current_state()
+
+    def set_start_state(self, msg):
+        self._g.set_start_state(conversions.msg_to_string(msg))
+
     def set_joint_value_target(self, name, value = None):
         """ Specify a target joint configuration for the group."""
         if value == None:

src/moveit_commander/robot.py

@@ -34,6 +34,7 @@
 
 from moveit_commander import MoveGroupCommander, MoveItCommanderException
 from moveit_ros_planning_interface import _moveit_robot_interface
+from moveit_msgs.msg import RobotState
 import conversions
 
 class RobotCommander(object):
@@ -129,6 +130,12 @@ def get_group_names(self):
         """Get the names of the groups defined for the robot"""
         return self._r.get_group_names()
 
+    def get_current_state(self):
+        """ Get a RobotState message describing the current state of the robot"""
+        s = RobotState()
+        s.deserialize(self._r.get_current_state())
+        return s
+
     def get_current_variable_values(self):
         """Get a dictionary mapping variable names to values. Note that a joint may consist of one or more variables """
         return self._r.get_current_variable_values()