robot.py
Go to the documentation of this file.
1 # Software License Agreement (BSD License)
2 #
3 # Copyright (c) 2013, Willow Garage, Inc.
4 # All rights reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions
8 # are met:
9 #
10 # * Redistributions of source code must retain the above copyright
11 # notice, this list of conditions and the following disclaimer.
12 # * Redistributions in binary form must reproduce the above
13 # copyright notice, this list of conditions and the following
14 # disclaimer in the documentation and/or other materials provided
15 # with the distribution.
16 # * Neither the name of Willow Garage, Inc. nor the names of its
17 # contributors may be used to endorse or promote products derived
18 # from this software without specific prior written permission.
19 #
20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 # POSSIBILITY OF SUCH DAMAGE.
32 #
33 # Author: Ioan Sucan
34 
35 from moveit_commander import MoveGroupCommander, MoveItCommanderException
36 from moveit_ros_planning_interface import _moveit_robot_interface
37 from moveit_msgs.msg import RobotState
38 from visualization_msgs.msg import MarkerArray
39 import moveit_commander.conversions as conversions
40 
41 
42 class RobotCommander(object):
43 
44  class Joint(object):
45  def __init__(self, robot, name):
46  self._robot = robot
47  self._name = name
48 
49  def name(self):
50  return self._name
51 
52  def variable_count(self):
53  """
54  @return number of the list that _Joint__get_joint_limits
55  methods returns.
56  @see: http://docs.ros.org/indigo/api/moveit_core/html/classmoveit_1_1core_1_1JointModel.html#details
57  for more about variable.
58  """
59  return len(self.__get_joint_limits())
60 
61  def bounds(self):
62  """
63  @return: Either a single list of min and max joint limits, or
64  a set of those lists, depending on the number of variables
65  available in this joint.
66  """
67  l = self.__get_joint_limits()
68  if len(l) == 1:
69  return l[0]
70  else:
71  return l
72 
73  def min_bound(self):
74  """
75  @return: Either a single min joint limit value, or
76  a set of min values, depending on the number of variables
77  available in this joint.
78  """
79  limits = self.__get_joint_limits()
80  if len(limits) == 1:
81  return limits[0][0]
82  else:
83  return [l[0] for l in limits]
84 
85  def max_bound(self):
86  """
87  @return: Either a single max joint limit value, or
88  a set of max values, depending on the number of variables
89  available in this joint.
90  """
91  limits = self.__get_joint_limits()
92  if len(limits) == 1:
93  return limits[0][1]
94  else:
95  return [l[1] for l in limits]
96 
97  def value(self):
98  """
99  @rtype float
100 
101  (Editor's comment by @130s) I doubt there's a case where this method goes into
102  "else" block, because get_current_joint_values always return a single list.
103 
104  cf. getCurrentJointValues https://github.com/ros-planning/moveit_ros/blob/8e819dda2b19462b8d0c5aacc69706c8a9d8d883/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp#L176
105  """
106  vals = self._robot._r.get_current_joint_values(self._name)
107  if len(vals) == 1:
108  return vals[0]
109  else:
110  return vals
111 
112  def move(self, position, wait=True):
113  """
114  @param position [float]: List of joint angles to achieve.
115  @param wait bool: If false, the commands gets operated asynchronously.
116  """
117  group = self._robot.get_default_owner_group(self.name())
118  if group is None:
119  raise MoveItCommanderException("There is no known group containing joint %s. Cannot move." % self._name)
120  gc = self._robot.get_group(group)
121  if gc is not None:
122  gc.set_joint_value_target(gc.get_current_joint_values())
123  gc.set_joint_value_target(self._name, position)
124  return gc.go(wait)
125  return False
126 
128  """
129  @return: A list of length of 2 that contains max and min positional
130  limits of the specified joint.
131  """
132  return self._robot._r.get_joint_limits(self._name)
133 
134  class Link(object):
135  def __init__(self, robot, name):
136  self._robot = robot
137  self._name = name
138 
139  def name(self):
140  return self._name
141 
142  def pose(self):
143  """
144  @rtype: geometry_msgs.Pose
145  """
146  return conversions.list_to_pose_stamped(self._robot._r.get_link_pose(self._name), self._robot.get_planning_frame())
147 
148  def __init__(self, robot_description="robot_description", ns=""):
149  self._robot_description = robot_description
150  self._ns = ns
151  self._r = _moveit_robot_interface.RobotInterface(robot_description, ns)
152  self._groups = {}
154 
156  """
157  Get the frame of reference in which planning is done (and environment
158  is maintained)
159  """
160  return self._r.get_planning_frame()
161 
162  def get_robot_markers(self, *args):
163  """Get a MarkerArray of the markers that make up this robot
164 
165  Usage:
166  (): get's all markers for current state
167  state (RobotState): gets markers for a particular state
168  values (dict): get markers with given values
169  values, links (dict, list): get markers with given values and these links
170  group (string): get all markers for a group
171  group, values (string, dict): get all markers for a group with desired values
172  """
173  mrkr = MarkerArray()
174  if not args:
175  conversions.msg_from_string(mrkr, self._r.get_robot_markers())
176  else:
177  if isinstance(args[0], RobotState):
178  msg_str = conversions.msg_to_string(args[0])
179  conversions.msg_from_string(mrkr, self._r.get_robot_markers(msg_str))
180  elif isinstance(args[0], dict):
181  conversions.msg_from_string(mrkr, self._r.get_robot_markers(*args))
182  elif isinstance(args[0], str):
183  conversions.msg_from_string(mrkr, self._r.get_group_markers(*args))
184  else:
185  raise MoveItCommanderException("Unexpected type")
186  return mrkr
187 
188  def get_root_link(self):
189  """Get the name of the root link of the robot model """
190  return self._r.get_robot_root_link()
191 
192  def get_joint_names(self, group=None):
193  """
194  Get the names of all the movable joints that make up a group
195  (mimic joints and fixed joints are excluded). If no group name is
196  specified, all joints in the robot model are returned, including
197  fixed and mimic joints.
198  """
199  if group is not None:
200  if self.has_group(group):
201  return self._r.get_group_joint_names(group)
202  else:
203  raise MoveItCommanderException("There is no group named %s" % group)
204  else:
205  return self._r.get_joint_names()
206 
207  def get_link_names(self, group=None):
208  """
209  Get the links that make up a group. If no group name is specified,
210  all the links in the robot model are returned.
211  """
212  if group is not None:
213  if self.has_group(group):
214  return self._r.get_group_link_names(group)
215  else:
216  raise MoveItCommanderException("There is no group named %s" % group)
217  else:
218  return self._r.get_link_names()
219 
220  def get_group_names(self):
221  """Get the names of the groups defined for the robot"""
222  return self._r.get_group_names()
223 
224  def get_current_state(self):
225  """ Get a RobotState message describing the current state of the robot"""
226  s = RobotState()
227  s.deserialize(self._r.get_current_state())
228  return s
229 
231  """
232  Get a dictionary mapping variable names to values.
233  Note that a joint may consist of one or more variables.
234  """
235  return self._r.get_current_variable_values()
236 
237  def get_joint(self, name):
238  """
239  @param name str: Name of movegroup
240  @rtype: moveit_commander.robot.Joint
241  @raise exception: MoveItCommanderException
242  """
243  if name in self.get_joint_names():
244  return self.Joint(self, name)
245  else:
246  raise MoveItCommanderException("There is no joint named %s" % name)
247 
248  def get_link(self, name):
249  """
250  @param name str: Name of movegroup
251  @rtype: moveit_commander.robot.Link
252  @raise exception: MoveItCommanderException
253  """
254  if name in self.get_link_names():
255  return self.Link(self, name)
256  else:
257  raise MoveItCommanderException("There is no link named %s" % name)
258 
259  def get_group(self, name):
260  """
261  @param name str: Name of movegroup
262  @rtype: moveit_commander.MoveGroupCommander
263  """
264  if not self._groups.has_key(name):
265  if not self.has_group(name):
266  raise MoveItCommanderException("There is no group named %s" % name)
267  self._groups[name] = MoveGroupCommander(name, self._robot_description, self._ns)
268  return self._groups[name]
269 
270  def has_group(self, name):
271  """
272  @param name str: Name of movegroup
273  @rtype: bool
274  """
275  return self._r.has_group(name)
276 
277  def get_default_owner_group(self, joint_name):
278  """
279  Get the name of the smallest group (fewest joints) that includes
280  the joint name specified as argument.
281  """
282  if not self._joint_owner_groups.has_key(joint_name):
283  group = None
284  for g in self.get_group_names():
285  if joint_name in self.get_joint_names(g):
286  if group is None:
287  group = g
288  else:
289  if len(self.get_link_names(g)) < len(self.get_link_names(group)):
290  group = g
291  self._joint_owner_groups[joint_name] = group
292  return self._joint_owner_groups[joint_name]
293 
294  def __getattr__(self, name):
295  """
296  We catch the names of groups, joints and links to allow easy access
297  to their properties.
298  """
299  if name in self.get_group_names():
300  return self.get_group(name)
301  elif name in self.get_joint_names():
302  return self.Joint(self, name)
303  elif name in self.get_link_names():
304  return self.Link(self, name)
305  else:
306  return object.__getattribute__(self, name)
def get_robot_markers(self, args)
Definition: robot.py:162
def get_default_owner_group(self, joint_name)
Definition: robot.py:277
def __init__(self, robot_description="robot_description", ns="")
Definition: robot.py:148
def __init__(self, robot, name)
Definition: robot.py:45
def get_link_names(self, group=None)
Definition: robot.py:207
def get_joint_names(self, group=None)
Definition: robot.py:192
def move(self, position, wait=True)
Definition: robot.py:112


moveit_commander
Author(s): Ioan Sucan
autogenerated on Wed Jul 10 2019 04:04:15