Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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(self.name())
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_groups = {}
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)