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 import conversions
39 
40 
41 class RobotCommander(object):
42 
43  class Joint(object):
44  def __init__(self, robot, name):
45  self._robot = robot
46  self._name = name
47 
48  def name(self):
49  return self._name
50 
51  def variable_count(self):
52  """
53  @return number of the list that _Joint__get_joint_limits
54  methods returns.
55  @see: http://docs.ros.org/indigo/api/moveit_core/html/classmoveit_1_1core_1_1JointModel.html#details
56  for more about variable.
57  """
58  return len(self.__get_joint_limits())
59 
60  def bounds(self):
61  """
62  @return: Either a single list of min and max joint limits, or
63  a set of those lists, depending on the number of variables
64  available in this joint.
65  """
66  l = self.__get_joint_limits()
67  if len(l) == 1:
68  return l[0]
69  else:
70  return l
71 
72  def min_bound(self):
73  """
74  @return: Either a single min joint limit value, or
75  a set of min values, depending on the number of variables
76  available in this joint.
77  """
78  limits = self.__get_joint_limits()
79  if len(limits) == 1:
80  return limits[0][0]
81  else:
82  return [l[0] for l in limits]
83 
84  def max_bound(self):
85  """
86  @return: Either a single max joint limit value, or
87  a set of max values, depending on the number of variables
88  available in this joint.
89  """
90  limits = self.__get_joint_limits()
91  if len(limits) == 1:
92  return limits[0][1]
93  else:
94  return [l[1] for l in limits]
95 
96  def value(self):
97  """
98  @rtype float
99 
100  (Editor's comment by @130s) I doubt there's a case where this method goes into
101  "else" block, because get_current_joint_values always return a single list.
102 
103  cf. getCurrentJointValues https://github.com/ros-planning/moveit_ros/blob/8e819dda2b19462b8d0c5aacc69706c8a9d8d883/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp#L176
104  """
105  vals = self._robot._r.get_current_joint_values(self._name)
106  if len(vals) == 1:
107  return vals[0]
108  else:
109  return vals
110 
111  def move(self, position, wait=True):
112  """
113  @param position [float]: List of joint angles to achieve.
114  @param wait bool: If false, the commands gets operated asynchronously.
115  """
116  group = self._robot.get_default_owner_group(self.name())
117  if group is None:
118  raise MoveItCommanderException("There is no known group containing joint %s. Cannot move." % self._name)
119  gc = self._robot.get_group(group)
120  if gc is not None:
121  gc.set_joint_value_target(gc.get_current_joint_values())
122  gc.set_joint_value_target(self._name, position)
123  return gc.go(wait)
124  return False
125 
127  """
128  @return: A list of length of 2 that contains max and min positional
129  limits of the specified joint.
130  """
131  return self._robot._r.get_joint_limits(self._name)
132 
133  class Link(object):
134  def __init__(self, robot, name):
135  self._robot = robot
136  self._name = name
137 
138  def name(self):
139  return self._name
140 
141  def pose(self):
142  """
143  @rtype: geometry_msgs.Pose
144  """
145  return conversions.list_to_pose_stamped(self._robot._r.get_link_pose(self._name), self._robot.get_planning_frame())
146 
147  def __init__(self, robot_description="robot_description"):
148  self._r = _moveit_robot_interface.RobotInterface(robot_description)
149  self._groups = {}
151 
153  """
154  Get the frame of reference in which planning is done (and environment
155  is maintained)
156  """
157  return self._r.get_planning_frame()
158 
159  def get_root_link(self):
160  """Get the name of the root link of the robot model """
161  return self._r.get_robot_root_link()
162 
163  def get_joint_names(self, group=None):
164  """
165  Get the names of all the movable joints that make up a group
166  (mimic joints and fixed joints are excluded). If no group name is
167  specified, all joints in the robot model are returned, including
168  fixed and mimic joints.
169  """
170  if group is not None:
171  if self.has_group(group):
172  return self._r.get_group_joint_names(group)
173  else:
174  raise MoveItCommanderException("There is no group named %s" % group)
175  else:
176  return self._r.get_joint_names()
177 
178  def get_link_names(self, group=None):
179  """
180  Get the links that make up a group. If no group name is specified,
181  all the links in the robot model are returned.
182  """
183  if group is not None:
184  if self.has_group(group):
185  return self._r.get_group_link_names(group)
186  else:
187  raise MoveItCommanderException("There is no group named %s" % group)
188  else:
189  return self._r.get_link_names()
190 
191  def get_group_names(self):
192  """Get the names of the groups defined for the robot"""
193  return self._r.get_group_names()
194 
195  def get_current_state(self):
196  """ Get a RobotState message describing the current state of the robot"""
197  s = RobotState()
198  s.deserialize(self._r.get_current_state())
199  return s
200 
202  """
203  Get a dictionary mapping variable names to values.
204  Note that a joint may consist of one or more variables.
205  """
206  return self._r.get_current_variable_values()
207 
208  def get_joint(self, name):
209  """
210  @param name str: Name of movegroup
211  @rtype: moveit_commander.robot.Joint
212  @raise exception: MoveItCommanderException
213  """
214  if name in self.get_joint_names():
215  return self.Joint(self, name)
216  else:
217  raise MoveItCommanderException("There is no joint named %s" % name)
218 
219  def get_link(self, name):
220  """
221  @param name str: Name of movegroup
222  @rtype: moveit_commander.robot.Link
223  @raise exception: MoveItCommanderException
224  """
225  if name in self.get_link_names():
226  return self.Link(self, name)
227  else:
228  raise MoveItCommanderException("There is no link named %s" % name)
229 
230  def get_group(self, name):
231  """
232  @param name str: Name of movegroup
233  @rtype: moveit_commander.MoveGroupCommander
234  """
235  if not self._groups.has_key(name):
236  if not self.has_group(name):
237  raise MoveItCommanderException("There is no group named %s" % name)
238  self._groups[name] = MoveGroupCommander(name)
239  return self._groups[name]
240 
241  def has_group(self, name):
242  """
243  @param name str: Name of movegroup
244  @rtype: bool
245  """
246  return self._r.has_group(name)
247 
248  def get_default_owner_group(self, joint_name):
249  """
250  Get the name of the smallest group (fewest joints) that includes
251  the joint name specified as argument.
252  """
253  if not self._joint_owner_groups.has_key(joint_name):
254  group = None
255  for g in self.get_group_names():
256  if joint_name in self.get_joint_names(g):
257  if group is None:
258  group = g
259  else:
260  if len(self.get_link_names(g)) < len(self.get_link_names(group)):
261  group = g
262  self._joint_owner_groups[joint_name] = group
263  return self._joint_owner_groups[joint_name]
264 
265  def __getattr__(self, name):
266  """
267  We catch the names of groups, joints and links to allow easy access
268  to their properties.
269  """
270  if name in self.get_group_names():
271  return self.get_group(name)
272  elif name in self.get_joint_names():
273  return self.Joint(self, name)
274  elif name in self.get_link_names():
275  return self.Link(self, name)
276  else:
277  return object.__getattribute__(self, name)
def get_default_owner_group(self, joint_name)
Definition: robot.py:248
def __init__(self, robot, name)
Definition: robot.py:44
def get_link_names(self, group=None)
Definition: robot.py:178
def __init__(self, robot_description="robot_description")
Definition: robot.py:147
def get_joint_names(self, group=None)
Definition: robot.py:163
def move(self, position, wait=True)
Definition: robot.py:111


moveit_commander
Author(s): Ioan Sucan
autogenerated on Mon Jan 15 2018 03:52:26