interpreter.py
Go to the documentation of this file.
1 # Software License Agreement (BSD License)
2 #
3 # Copyright (c) 2012, 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 import rospy
36 from moveit_commander import RobotCommander, MoveGroupCommander, PlanningSceneInterface, MoveItCommanderException
37 from geometry_msgs.msg import Pose, PoseStamped
38 import tf
39 import re
40 import time
41 import os.path
42 
44  FAIL = 1
45  WARN = 2
46  SUCCESS = 3
47  INFO = 4
48  DEBUG = 5
49 
51  """
52  Interpreter for simple commands
53  """
54 
55  DEFAULT_FILENAME = "move_group.cfg"
56  GO_DIRS = {"up" : (2,1), "down" : (2, -1), "z" : (2, 1),
57  "left" : (1, 1), "right" : (1, -1), "y" : (1, 1),
58  "forward" : (0, 1), "backward" : (0, -1), "back" : (0, -1), "x" : (0, 1) }
59 
60  def __init__(self):
61  self._gdict = {}
62  self._group_name = ""
63  self._prev_group_name = ""
64  self._planning_scene_interface = PlanningSceneInterface()
65  self._robot = RobotCommander()
66  self._last_plan = None
67  self._db_host = None
68  self._db_port = 33829
69  self._trace = False
70 
71  def get_active_group(self):
72  if len(self._group_name) > 0:
73  return self._gdict[self._group_name]
74  else:
75  return None
76 
77  def get_loaded_groups(self):
78  return self._gdict.keys()
79 
80  def execute(self, cmd):
81  cmd = self.resolve_command_alias(cmd)
82  result = self.execute_generic_command(cmd)
83  if result != None:
84  return result
85  else:
86  if len(self._group_name) > 0:
87  return self.execute_group_command(self._gdict[self._group_name], cmd)
88  else:
89  return (MoveGroupInfoLevel.INFO, self.get_help() + "\n\nNo groups initialized yet. You must call the 'use' or the 'load' command first.\n")
90 
91  def execute_generic_command(self, cmd):
92  if os.path.isfile("cmd/" + cmd):
93  cmd = "load cmd/" + cmd
94  cmdlow = cmd.lower()
95  if cmdlow.startswith("use"):
96  if cmdlow == "use":
97  return (MoveGroupInfoLevel.INFO, "\n".join(self.get_loaded_groups()))
98  clist = cmd.split()
99  clist[0] = clist[0].lower()
100  if len(clist) == 2:
101  if clist[0] == "use":
102  if clist[1] == "previous":
103  clist[1] = self._prev_group_name
104  if len(clist[1]) == 0:
105  return (MoveGroupInfoLevel.DEBUG, "OK")
106  if self._gdict.has_key(clist[1]):
107  self._prev_group_name = self._group_name
108  self._group_name = clist[1]
109  return (MoveGroupInfoLevel.DEBUG, "OK")
110  else:
111  try:
112  mg = MoveGroupCommander(clist[1])
113  self._gdict[clist[1]] = mg
114  self._group_name = clist[1]
115  return (MoveGroupInfoLevel.DEBUG, "OK")
116  except MoveItCommanderException as e:
117  return (MoveGroupInfoLevel.FAIL, "Initializing " + clist[1] + ": " + str(e))
118  except:
119  return (MoveGroupInfoLevel.FAIL, "Unable to initialize " + clist[1])
120  elif cmdlow.startswith("trace"):
121  if cmdlow == "trace":
122  return (MoveGroupInfoLevel.INFO, "trace is on" if self._trace else "trace is off")
123  clist = cmdlow.split()
124  if clist[0] == "trace" and clist[1] == "on":
125  self._trace = True
126  return (MoveGroupInfoLevel.DEBUG, "OK")
127  if clist[0] == "trace" and clist[1] == "off":
128  self._trace = False
129  return (MoveGroupInfoLevel.DEBUG, "OK")
130  elif cmdlow.startswith("load"):
131  filename = self.DEFAULT_FILENAME
132  clist = cmd.split()
133  if len(clist) > 2:
134  return (MoveGroupInfoLevel.WARN, "Unable to parse load command")
135  if len(clist) == 2:
136  filename = clist[1]
137  if not os.path.isfile(filename):
138  filename = "cmd/" + filename
139  line_num = 0
140  line_content = ""
141  try:
142  f = open(filename, 'r')
143  for line in f:
144  line_num += 1
145  line = line.rstrip()
146  line_content = line
147  if self._trace:
148  print ("%s:%d: %s" % (filename, line_num, line_content))
149  comment = line.find("#")
150  if comment != -1:
151  line = line[0:comment].rstrip()
152  if line != "":
153  self.execute(line.rstrip())
154  line_content = ""
155  return (MoveGroupInfoLevel.DEBUG, "OK")
156  except MoveItCommanderException as e:
157  if line_num > 0:
158  return (MoveGroupInfoLevel.WARN, "Error at %s:%d: %s\n%s" % (filename, line_num, line_content, str(e)))
159  else:
160  return (MoveGroupInfoLevel.WARN, "Processing " + filename + ": " + str(e))
161  except:
162  if line_num > 0:
163  return (MoveGroupInfoLevel.WARN, "Error at %s:%d: %s" % (filename, line_num, line_content))
164  else:
165  return (MoveGroupInfoLevel.WARN, "Unable to load " + filename)
166  elif cmdlow.startswith("save"):
167  filename = self.DEFAULT_FILENAME
168  clist = cmd.split()
169  if len(clist) > 2:
170  return (MoveGroupInfoLevel.WARN, "Unable to parse save command")
171  if len(clist) == 2:
172  filename = clist[1]
173  try:
174  f = open(filename, 'w')
175  for gr in self._gdict.keys():
176  f.write("use " + gr + "\n")
177  known = self._gdict[gr].get_remembered_joint_values()
178  for v in known.keys():
179  f.write(v + " = [" + " ".join([str(x) for x in known[v]]) + "]\n")
180  if self._db_host != None:
181  f.write("database " + self._db_host + " " + str(self._db_port) + "\n")
182  return (MoveGroupInfoLevel.DEBUG, "OK")
183  except:
184  return (MoveGroupInfoLevel.WARN, "Unable to save " + filename)
185  else:
186  return None
187 
188  def execute_group_command(self, g, cmdorig):
189  cmd = cmdorig.lower()
190 
191  if cmd == "stop":
192  g.stop()
193  return (MoveGroupInfoLevel.DEBUG, "OK")
194 
195  if cmd == "id":
196  return (MoveGroupInfoLevel.INFO, g.get_name())
197 
198  if cmd == "help":
199  return (MoveGroupInfoLevel.INFO, self.get_help())
200 
201  if cmd == "vars":
202  known = g.get_remembered_joint_values()
203  return (MoveGroupInfoLevel.INFO, "[" + " ".join(known.keys()) + "]")
204 
205  if cmd == "joints":
206  joints = g.get_joints()
207  return (MoveGroupInfoLevel.INFO, "\n" + "\n".join([str(i) + " = " + joints[i] for i in range(len(joints))]) + "\n")
208 
209  if cmd == "show":
210  return self.command_show(g)
211 
212  if cmd == "current":
213  return self.command_current(g)
214 
215  if cmd == "ground":
216  pose = PoseStamped()
217  pose.pose.position.x = 0
218  pose.pose.position.y = 0
219  # offset such that the box is 0.1 mm below ground (to prevent collision with the robot itself)
220  pose.pose.position.z = -0.0501
221  pose.pose.orientation.x = 0
222  pose.pose.orientation.y = 0
223  pose.pose.orientation.z = 0
224  pose.pose.orientation.w = 1
225  pose.header.stamp = rospy.get_rostime()
226  pose.header.frame_id = self._robot.get_root_link()
227  self._planning_scene_interface.attach_box(self._robot.get_root_link(), "ground", pose, (3, 3, 0.1))
228  return (MoveGroupInfoLevel.INFO, "Added ground")
229 
230  if cmd == "eef":
231  if len(g.get_end_effector_link()) > 0:
232  return (MoveGroupInfoLevel.INFO, g.get_end_effector_link())
233  else:
234  return (MoveGroupInfoLevel.INFO, "There is no end effector defined")
235 
236  if cmd == "database":
237  if self._db_host == None:
238  return (MoveGroupInfoLevel.INFO, "Not connected to a database")
239  else:
240  return (MoveGroupInfoLevel.INFO, "Connected to " + self._db_host + ":" + str(self._db_port))
241  if cmd == "plan":
242  if self._last_plan != None:
243  return (MoveGroupInfoLevel.INFO, str(self._last_plan))
244  else:
245  return (MoveGroupInfoLevel.INFO, "No previous plan")
246 
247  if cmd == "constrain":
248  g.clear_path_constraints()
249  return (MoveGroupInfoLevel.SUCCESS, "Cleared path constraints")
250 
251  if cmd == "tol" or cmd == "tolerance":
252  return (MoveGroupInfoLevel.INFO, "Joint = %0.6g, Position = %0.6g, Orientation = %0.6g" % g.get_goal_tolerance())
253 
254  if cmd == "time":
255  return (MoveGroupInfoLevel.INFO, str(g.get_planning_time()))
256 
257  if cmd == "execute":
258  if self._last_plan != None:
259  if g.execute(self._last_plan):
260  return (MoveGroupInfoLevel.SUCCESS, "Plan submitted for execution")
261  else:
262  return (MoveGroupInfoLevel.WARN, "Failed to submit plan for execution")
263  else:
264  return (MoveGroupInfoLevel.WARN, "No motion plan computed")
265 
266  # see if we have assignment between variables
267  assign_match = re.match(r"^(\w+)\s*=\s*(\w+)$", cmd)
268  if assign_match:
269  known = g.get_remembered_joint_values()
270  if known.has_key(assign_match.group(2)):
271  g.remember_joint_values(assign_match.group(1), known[assign_match.group(2)])
272  return (MoveGroupInfoLevel.SUCCESS, assign_match.group(1) + " is now the same as " + assign_match.group(2))
273  else:
274  return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
275 
276  # see if we have assignment of matlab-like vector syntax
277  set_match = re.match(r"^(\w+)\s*=\s*\[([\d\.e\-\+\s]+)\]$", cmd)
278  if set_match:
279  try:
280  g.remember_joint_values(set_match.group(1), [float(x) for x in set_match.group(2).split()])
281  return (MoveGroupInfoLevel.SUCCESS, "Remembered joint values [" + set_match.group(2) + "] under the name " + set_match.group(1))
282  except:
283  return (MoveGroupInfoLevel.WARN, "Unable to parse joint value [" + set_match.group(2) + "]")
284 
285  # see if we have assignment of matlab-like element update
286  component_match = re.match(r"^(\w+)\s*\[\s*(\d+)\s*\]\s*=\s*([\d\.e\-\+]+)$", cmd)
287  if component_match:
288  known = g.get_remembered_joint_values()
289  if known.has_key(component_match.group(1)):
290  try:
291  val = known[component_match.group(1)]
292  val[int(component_match.group(2))] = float(component_match.group(3))
293  g.remember_joint_values(component_match.group(1), val)
294  return (MoveGroupInfoLevel.SUCCESS, "Updated " + component_match.group(1) + "[" + component_match.group(2) + "]")
295  except:
296  return (MoveGroupInfoLevel.WARN, "Unable to parse index or value in '" + cmd +"'")
297  else:
298  return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
299 
300  clist = cmdorig.split()
301  clist[0] = clist[0].lower()
302 
303  # if this is an unknown one-word command, it is probably a variable
304  if len(clist) == 1:
305  known = g.get_remembered_joint_values()
306  if known.has_key(cmd):
307  return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[cmd]]) + "]")
308  else:
309  return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
310 
311  # command with one argument
312  if len(clist) == 2:
313  if clist[0] == "go":
314  self._last_plan = None
315  if clist[1] == "rand" or clist[1] == "random":
316  vals = g.get_random_joint_values()
317  g.set_joint_value_target(vals)
318  if g.go():
319  return (MoveGroupInfoLevel.SUCCESS, "Moved to random target [" + " ".join([str(x) for x in vals]) + "]")
320  else:
321  return (MoveGroupInfoLevel.FAIL, "Failed while moving to random target [" + " ".join([str(x) for x in vals]) + "]")
322  else:
323  try:
324  g.set_named_target(clist[1])
325  if g.go():
326  return (MoveGroupInfoLevel.SUCCESS, "Moved to " + clist[1])
327  else:
328  return (MoveGroupInfoLevel.FAIL, "Failed while moving to " + clist[1])
329  except MoveItCommanderException as e:
330  return (MoveGroupInfoLevel.WARN, "Going to " + clist[1] + ": " + str(e))
331  except:
332  return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown")
333  if clist[0] == "plan":
334  self._last_plan = None
335  vals = None
336  if clist[1] == "rand" or clist[1] == "random":
337  vals = g.get_random_joint_values()
338  g.set_joint_value_target(vals)
339  self._last_plan = g.plan()
340  else:
341  try:
342  g.set_named_target(clist[1])
343  self._last_plan = g.plan()
344  except MoveItCommanderException as e:
345  return (MoveGroupInfoLevel.WARN, "Planning to " + clist[1] + ": " + str(e))
346  except:
347  return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown")
348  if self._last_plan != None:
349  if len(self._last_plan.joint_trajectory.points) == 0 and len(self._last_plan.multi_dof_joint_trajectory.points) == 0:
350  self._last_plan = None
351  dest_str = clist[1]
352  if vals != None:
353  dest_str = "[" + " ".join([str(x) for x in vals]) + "]"
354  if self._last_plan != None:
355  return (MoveGroupInfoLevel.SUCCESS, "Planned to " + dest_str)
356  else:
357  return (MoveGroupInfoLevel.FAIL, "Failed while planning to " + dest_str)
358  elif clist[0] == "pick":
359  self._last_plan = None
360  if g.pick(clist[1]):
361  return (MoveGroupInfoLevel.SUCCESS, "Picked object " + clist[1])
362  else:
363  return (MoveGroupInfoLevel.FAIL, "Failed while trying to pick object " + clist[1])
364  elif clist[0] == "place":
365  self._last_plan = None
366  if g.place(clist[1]):
367  return (MoveGroupInfoLevel.SUCCESS, "Placed object " + clist[1])
368  else:
369  return (MoveGroupInfoLevel.FAIL, "Failed while trying to place object " + clist[1])
370  elif clist[0] == "planner":
371  g.set_planner_id(clist[1])
372  return (MoveGroupInfoLevel.SUCCESS, "Planner is now " + clist[1])
373  elif clist[0] == "record" or clist[0] == "rec":
374  g.remember_joint_values(clist[1])
375  return (MoveGroupInfoLevel.SUCCESS, "Remembered current joint values under the name " + clist[1])
376  elif clist[0] == "rand" or clist[0] == "random":
377  g.remember_joint_values(clist[1], g.get_random_joint_values())
378  return (MoveGroupInfoLevel.SUCCESS, "Remembered random joint values under the name " + clist[1])
379  elif clist[0] == "del" or clist[0] == "delete":
380  g.forget_joint_values(clist[1])
381  return (MoveGroupInfoLevel.SUCCESS, "Forgot joint values under the name " + clist[1])
382  elif clist[0] == "show":
383  known = g.get_remembered_joint_values()
384  if known.has_key(clist[1]):
385  return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[clist[1]]]) + "]")
386  else:
387  return (MoveGroupInfoLevel.WARN, "Joint values for " + clist[1] + " are not known")
388  elif clist[0] == "tol" or clist[0] == "tolerance":
389  try:
390  g.set_goal_tolerance(float(clist[1]))
391  return (MoveGroupInfoLevel.SUCCESS, "OK")
392  except:
393  return (MoveGroupInfoLevel.WARN, "Unable to parse tolerance value '" + clist[1] + "'")
394  elif clist[0] == "time":
395  try:
396  g.set_planning_time(float(clist[1]))
397  return (MoveGroupInfoLevel.SUCCESS, "OK")
398  except:
399  return (MoveGroupInfoLevel.WARN, "Unable to parse planning duration value '" + clist[1] + "'")
400  elif clist[0] == "constrain":
401  try:
402  g.set_path_constraints(clist[1])
403  return (MoveGroupInfoLevel.SUCCESS, "OK")
404  except:
405  if self._db_host != None:
406  return (MoveGroupInfoLevel.WARN, "Constraint " + clist[1] + " is not known.")
407  else:
408  return (MoveGroupInfoLevel.WARN, "Not connected to a database.")
409  elif clist[0] == "wait":
410  try:
411  time.sleep(float(clist[1]))
412  return (MoveGroupInfoLevel.SUCCESS, clist[1] + " seconds passed")
413  except:
414  return (MoveGroupInfoLevel.WARN, "Unable to wait '" + clist[1] + "' seconds")
415  elif clist[0] == "database":
416  try:
417  g.set_constraints_database(clist[1], self._db_port)
418  self._db_host = clist[1]
419  return (MoveGroupInfoLevel.SUCCESS, "Connected to " + self._db_host + ":" + str(self._db_port))
420  except:
421  return (MoveGroupInfoLevel.WARN, "Unable to connect to '" + clist[1] + ":" + str(self._db_port) + "'")
422  else:
423  return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
424 
425  if len(clist) == 3:
426  if clist[0] == "go" and self.GO_DIRS.has_key(clist[1]):
427  self._last_plan = None
428  try:
429  offset = float(clist[2])
430  (axis, factor) = self.GO_DIRS[clist[1]]
431  return self.command_go_offset(g, offset, factor, axis, clist[1])
432  except MoveItCommanderException as e:
433  return (MoveGroupInfoLevel.WARN, "Going " + clist[1] + ": " + str(e))
434  except:
435  return (MoveGroupInfoLevel.WARN, "Unable to parse distance '" + clist[2] + "'")
436  elif clist[0] == "allow" and (clist[1] == "look" or clist[1] == "looking"):
437  if clist[2] == "1" or clist[2] == "true" or clist[2] == "T" or clist[2] == "True":
438  g.allow_looking(True)
439  else:
440  g.allow_looking(False)
441  return (MoveGroupInfoLevel.DEBUG, "OK")
442  elif clist[0] == "allow" and (clist[1] == "replan" or clist[1] == "replanning"):
443  if clist[2] == "1" or clist[2] == "true" or clist[2] == "T" or clist[2] == "True":
444  g.allow_replanning(True)
445  else:
446  g.allow_replanning(False)
447  return (MoveGroupInfoLevel.DEBUG, "OK")
448  elif clist[0] == "database":
449  try:
450  g.set_constraints_database(clist[1], int(clist[2]))
451  self._db_host = clist[1]
452  self._db_port = int(clist[2])
453  return (MoveGroupInfoLevel.SUCCESS, "Connected to " + self._db_host + ":" + str(self._db_port))
454  except:
455  self._db_host = None
456  return (MoveGroupInfoLevel.WARN, "Unable to connect to '" + clist[1] + ":" + clist[2] + "'")
457  if len(clist) == 4:
458  if clist[0] == "rotate":
459  try:
460  g.set_rpy_target([float(x) for x in clist[1:]])
461  if g.go():
462  return (MoveGroupInfoLevel.SUCCESS, "Rotation complete")
463  else:
464  return (MoveGroupInfoLevel.FAIL, "Failed while rotating to " + " ".join(clist[1:]))
465  except MoveItCommanderException as e:
466  return (MoveGroupInfoLevel.WARN, str(e))
467  except:
468  return (MoveGroupInfoLevel.WARN, "Unable to parse X-Y-Z rotation values '" + " ".join(clist[1:]) + "'")
469  if len(clist) >= 7:
470  if clist[0] == "go":
471  self._last_plan = None
472  approx = False
473  if (len(clist) > 7):
474  if (clist[7] == "approx" or clist[7] == "approximate"):
475  approx = True
476  try:
477  p = Pose()
478  p.position.x = float(clist[1])
479  p.position.y = float(clist[2])
480  p.position.z = float(clist[3])
481  q = tf.transformations.quaternion_from_euler(float(clist[4]), float(clist[5]), float(clist[6]))
482  p.orientation.x = q[0]
483  p.orientation.y = q[1]
484  p.orientation.z = q[2]
485  p.orientation.w = q[3]
486  if approx:
487  g.set_joint_value_target(p, True)
488  else:
489  g.set_pose_target(p)
490  if g.go():
491  return (MoveGroupInfoLevel.SUCCESS, "Moved to pose target\n%s\n" % (str(p)))
492  else:
493  return (MoveGroupInfoLevel.FAIL, "Failed while moving to pose \n%s\n" % (str(p)))
494  except MoveItCommanderException as e:
495  return (MoveGroupInfoLevel.WARN, "Going to pose target: %s" % (str(e)))
496  except:
497  return (MoveGroupInfoLevel.WARN, "Unable to parse pose '" + " ".join(clist[1:]) + "'")
498 
499  return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
500 
501  def command_show(self, g):
502  known = g.get_remembered_joint_values()
503  res = []
504  for k in known.keys():
505  res.append(k + " = [" + " ".join([str(x) for x in known[k]]) + "]")
506  return (MoveGroupInfoLevel.INFO, "\n".join(res))
507 
508  def command_current(self, g):
509  res = "joints = [" + " ".join([str(x) for x in g.get_current_joint_values()]) + "]"
510  if len(g.get_end_effector_link()) > 0:
511  res = res + "\n" + g.get_end_effector_link() + " pose = [\n" + str(g.get_current_pose()) + " ]"
512  res = res + "\n" + g.get_end_effector_link() + " RPY = " + str(g.get_current_rpy())
513  return (MoveGroupInfoLevel.INFO, res)
514 
515  def command_go_offset(self, g, offset, factor, dimension_index, direction_name):
516  if g.has_end_effector_link():
517  try:
518  g.shift_pose_target(dimension_index, factor * offset)
519  if g.go():
520  return (MoveGroupInfoLevel.SUCCESS, "Moved " + direction_name + " by " + str(offset) + " m")
521  else:
522  return (MoveGroupInfoLevel.FAIL, "Failed while moving " + direction_name)
523  except MoveItCommanderException as e:
524  return (MoveGroupInfoLevel.WARN, str(e))
525  except:
526  return (MoveGroupInfoLevel.WARN, "Unable to process pose update")
527  else:
528  return (MoveGroupInfoLevel.WARN, "No known end effector. Cannot move " + direction_name)
529 
530  def resolve_command_alias(self, cmdorig):
531  cmd = cmdorig.lower()
532  if cmd == "which":
533  return "id"
534  if cmd == "groups":
535  return "use"
536  return cmdorig
537 
538  def get_help(self):
539  res = []
540  res.append("Known commands:")
541  res.append(" help show this screen")
542  res.append(" allow looking <true|false> enable/disable looking around")
543  res.append(" allow replanning <true|false> enable/disable replanning")
544  res.append(" constrain clear path constraints")
545  res.append(" constrain <name> use the constraint <name> as a path constraint")
546  res.append(" current show the current state of the active group")
547  res.append(" database display the current database connection (if any)")
548  res.append(" delete <name> forget the joint values under the name <name>")
549  res.append(" eef print the name of the end effector attached to the current group")
550  res.append(" execute execute a previously computed motion plan")
551  res.append(" go <name> plan and execute a motion to the state <name>")
552  res.append(" go rand plan and execute a motion to a random state")
553  res.append(" go <dir> <dx>| plan and execute a motion in direction up|down|left|right|forward|backward for distance <dx>")
554  res.append(" ground add a ground plane to the planning scene")
555  res.append(" id|which display the name of the group that is operated on")
556  res.append(" joints display names of the joints in the active group")
557  res.append(" load [<file>] load a set of interpreted commands from a file")
558  res.append(" pick <name> pick up object <name>")
559  res.append(" place <name> place object <name>")
560  res.append(" plan <name> plan a motion to the state <name>")
561  res.append(" plan rand plan a motion to a random state")
562  res.append(" planner <name> use planner <name> to plan next motion")
563  res.append(" record <name> record the current joint values under the name <name>")
564  res.append(" rotate <x> <y> <z> plan and execute a motion to a specified orientation (about the X,Y,Z axes)")
565  res.append(" save [<file>] save the currently known variables as a set of commands")
566  res.append(" show display the names and values of the known states")
567  res.append(" show <name> display the value of a state")
568  res.append(" stop stop the active group")
569  res.append(" time show the configured allowed planning time")
570  res.append(" time <val> set the allowed planning time")
571  res.append(" tolerance show the tolerance for reaching the goal region")
572  res.append(" tolerance <val> set the tolerance for reaching the goal region")
573  res.append(" trace <on|off> enable/disable replanning or looking around")
574  res.append(" use <name> switch to using the group named <name> (and load it if necessary)")
575  res.append(" use|groups show the group names that are already loaded")
576  res.append(" vars display the names of the known states")
577  res.append(" wait <dt> sleep for <dt> seconds")
578  res.append(" x = y assign the value of y to x")
579  res.append(" x = [v1 v2...] assign a vector of values to x")
580  res.append(" x[idx] = val assign a value to dimension idx of x")
581  return "\n".join(res)
582 
583  def get_keywords(self):
584  known_vars = []
585  known_constr = []
586  if self.get_active_group() != None:
587  known_vars = self.get_active_group().get_remembered_joint_values().keys()
588  known_constr = self.get_active_group().get_known_constraints()
589  groups = self._robot.get_group_names()
590  return {'go':['up', 'down', 'left', 'right', 'backward', 'forward', 'random'] + known_vars,
591  'help':[],
592  'record':known_vars,
593  'show':known_vars,
594  'wait':[],
595  'delete':known_vars,
596  'database': [],
597  'current':[],
598  'use':groups,
599  'load':[],
600  'save':[],
601  'pick':[],
602  'place':[],
603  'plan':known_vars,
604  'allow':['replanning', 'looking'],
605  'constrain':known_constr,
606  'vars':[],
607  'joints':[],
608  'tolerance':[],
609  'time':[],
610  'eef':[],
611  'execute':[],
612  'ground':[],
613  'id':[]}
def command_go_offset(self, g, offset, factor, dimension_index, direction_name)
Definition: interpreter.py:515


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