robot.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2013, Willow Garage, Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions
00008 # are met:
00009 #
00010 #  * Redistributions of source code must retain the above copyright
00011 #    notice, this list of conditions and the following disclaimer.
00012 #  * Redistributions in binary form must reproduce the above
00013 #    copyright notice, this list of conditions and the following
00014 #    disclaimer in the documentation and/or other materials provided
00015 #    with the distribution.
00016 #  * Neither the name of Willow Garage, Inc. nor the names of its
00017 #    contributors may be used to endorse or promote products derived
00018 #    from this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 # POSSIBILITY OF SUCH DAMAGE.
00032 #
00033 # Author: Ioan Sucan
00034 
00035 from moveit_commander import MoveGroupCommander, MoveItCommanderException
00036 from moveit_ros_planning_interface import _moveit_robot_interface
00037 from moveit_msgs.msg import RobotState
00038 import conversions
00039 
00040 
00041 class RobotCommander(object):
00042 
00043     class Joint(object):
00044         def __init__(self, robot, name):
00045             self._robot = robot
00046             self._name = name
00047 
00048         def name(self):
00049             return self._name
00050 
00051         def variable_count(self):
00052             """
00053             @return number of the list that _Joint__get_joint_limits
00054                     methods returns.
00055             @see: http://docs.ros.org/indigo/api/moveit_core/html/classmoveit_1_1core_1_1JointModel.html#details
00056                   for more about variable.
00057             """
00058             return len(self.__get_joint_limits())
00059 
00060         def bounds(self):
00061             """
00062             @return: Either a single list of min and max joint limits, or
00063                      a set of those lists, depending on the number of variables
00064                      available in this joint.
00065             """
00066             l = self.__get_joint_limits()
00067             if len(l) == 1:
00068                 return l[0]
00069             else:
00070                 return l
00071 
00072         def min_bound(self):
00073             """
00074             @return: Either a single min joint limit value, or
00075                      a set of min values, depending on the number of variables
00076                      available in this joint.
00077             """
00078             limits = self.__get_joint_limits()
00079             if len(limits) == 1:
00080                 return limits[0][0]
00081             else:
00082                 return [l[0] for l in limits]
00083 
00084         def max_bound(self):
00085             """
00086             @return: Either a single max joint limit value, or
00087                      a set of max values, depending on the number of variables
00088                      available in this joint.
00089             """
00090             limits = self.__get_joint_limits()
00091             if len(limits) == 1:
00092                 return limits[0][1]
00093             else:
00094                 return [l[1] for l in limits]
00095 
00096         def value(self):
00097             """
00098             @rtype float
00099 
00100             (Editor's comment by @130s) I doubt there's a case where this method goes into
00101             "else" block, because get_current_joint_values always return a single list.
00102 
00103             cf. getCurrentJointValues https://github.com/ros-planning/moveit_ros/blob/8e819dda2b19462b8d0c5aacc69706c8a9d8d883/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp#L176
00104             """
00105             vals = self._robot._r.get_current_joint_values(self._name)
00106             if len(vals) == 1:
00107                 return vals[0]
00108             else:
00109                 return vals
00110 
00111         def move(self, position, wait=True):
00112             """
00113             @param position [float]: List of joint angles to achieve.
00114             @param wait bool: If false, the commands gets operated asynchronously.
00115             """
00116             group = self._robot.get_default_owner_group()
00117             if group is None:
00118                 raise MoveItCommanderException("There is no known group containing joint %s. Cannot move." % self._name)
00119             gc = self._robot.get_group(group)
00120             if gc is not None:
00121                 gc.set_joint_value_target(gc.get_current_joint_values())
00122                 gc.set_joint_value_target(self._name, position)
00123                 return gc.go(wait)
00124             return False
00125 
00126         def __get_joint_limits(self):
00127             """
00128             @return: A list of length of 2 that contains max and min positional
00129                      limits of the specified joint.
00130             """
00131             return self._robot._r.get_joint_limits(self._name)
00132 
00133     class Link(object):
00134         def __init__(self, robot, name):
00135             self._robot = robot
00136             self._name = name
00137 
00138         def name(self):
00139             return self._name
00140 
00141         def pose(self):
00142             """
00143             @rtype: geometry_msgs.Pose
00144             """
00145             return conversions.list_to_pose_stamped(self._robot._r.get_link_pose(self._name), self._robot.get_planning_frame())
00146 
00147     def __init__(self, robot_description="robot_description"):
00148         self._r = _moveit_robot_interface.RobotInterface(robot_description)
00149         self._groups = {}
00150         self._joint_owner_group = {}
00151 
00152     def get_planning_frame(self):
00153         """
00154         Get the frame of reference in which planning is done (and environment
00155         is maintained)
00156         """
00157         return self._r.get_planning_frame()
00158 
00159     def get_root_link(self):
00160         """Get the name of the root link of the robot model """
00161         return self._r.get_robot_root_link()
00162 
00163     def get_joint_names(self, group=None):
00164         """
00165         Get the names of all the movable joints that make up a group
00166         (mimic joints and fixed joints are excluded). If no group name is
00167         specified, all joints in the robot model are returned, including
00168         fixed and mimic joints.
00169         """
00170         if group is not None:
00171             if self.has_group(group):
00172                 return self._r.get_group_joint_names(group)
00173             else:
00174                 raise MoveItCommanderException("There is no group named %s" % group)
00175         else:
00176             return self._r.get_joint_names()
00177 
00178     def get_link_names(self, group=None):
00179         """
00180         Get the links that make up a group. If no group name is specified,
00181         all the links in the robot model are returned.
00182         """
00183         if group is not None:
00184             if self.has_group(group):
00185                 return self._r.get_group_link_names(group)
00186             else:
00187                 raise MoveItCommanderException("There is no group named %s" % group)
00188         else:
00189             return self._r.get_link_names()
00190 
00191     def get_group_names(self):
00192         """Get the names of the groups defined for the robot"""
00193         return self._r.get_group_names()
00194 
00195     def get_current_state(self):
00196         """ Get a RobotState message describing the current state of the robot"""
00197         s = RobotState()
00198         s.deserialize(self._r.get_current_state())
00199         return s
00200 
00201     def get_current_variable_values(self):
00202         """
00203         Get a dictionary mapping variable names to values.
00204         Note that a joint may consist of one or more variables.
00205         """
00206         return self._r.get_current_variable_values()
00207 
00208     def get_joint(self, name):
00209         """
00210         @param name str: Name of movegroup
00211         @rtype: moveit_commander.robot.Joint
00212         @raise exception: MoveItCommanderException
00213         """
00214         if name in self.get_joint_names():
00215             return self.Joint(self, name)
00216         else:
00217             raise MoveItCommanderException("There is no joint named %s" % name)
00218 
00219     def get_link(self, name):
00220         """
00221         @param name str: Name of movegroup
00222         @rtype: moveit_commander.robot.Link
00223         @raise exception: MoveItCommanderException
00224         """
00225         if name in self.get_link_names():
00226             return self.Link(self, name)
00227         else:
00228             raise MoveItCommanderException("There is no link named %s" % name)
00229 
00230     def get_group(self, name):
00231         """
00232         @param name str: Name of movegroup
00233         @rtype: moveit_commander.MoveGroupCommander
00234         """
00235         if not self._groups.has_key(name):
00236             if not self.has_group(name):
00237                 raise MoveItCommanderException("There is no group named %s" % name)
00238             self._groups[name] = MoveGroupCommander(name)
00239         return self._groups[name]
00240 
00241     def has_group(self, name):
00242         """
00243         @param name str: Name of movegroup
00244         @rtype: bool
00245         """
00246         return self._r.has_group(name)
00247 
00248     def get_default_owner_group(self, joint_name):
00249         """
00250         Get the name of the smallest group (fewest joints) that includes
00251         the joint name specified as argument.
00252         """
00253         if not self._joint_owner_groups.has_key(joint_name):
00254             group = None
00255             for g in self.get_group_names():
00256                 if joint_name in self.get_joint_names(g):
00257                     if group is None:
00258                         group = g
00259                     else:
00260                         if len(self.get_link_names(g)) < len(self.get_link_names(group)):
00261                             group = g
00262             self._joint_owner_groups[joint_name] = group
00263         return self._joint_owner_groups[joint_name]
00264 
00265     def __getattr__(self, name):
00266         """
00267         We catch the names of groups, joints and links to allow easy access
00268         to their properties.
00269         """
00270         if name in self.get_group_names():
00271             return self.get_group(name)
00272         elif name in self.get_joint_names():
00273             return self.Joint(self, name)
00274         elif name in self.get_link_names():
00275             return self.Link(self, name)
00276         else:
00277             return object.__getattribute__(self, name)


moveit_commander
Author(s): Ioan Sucan
autogenerated on Mon Jul 24 2017 02:22:10