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 (
37  RobotCommander,
38  MoveGroupCommander,
39  PlanningSceneInterface,
40  MoveItCommanderException,
41 )
42 from geometry_msgs.msg import Pose, PoseStamped
43 import tf
44 import re
45 import time
46 import os.path
47 
48 
49 class MoveGroupInfoLevel:
50  FAIL = 1
51  WARN = 2
52  SUCCESS = 3
53  INFO = 4
54  DEBUG = 5
55 
56 
57 class MoveGroupCommandInterpreter(object):
58  """
59  Interpreter for simple commands
60  """
61 
62  DEFAULT_FILENAME = "move_group.cfg"
63  GO_DIRS = {
64  "up": (2, 1),
65  "down": (2, -1),
66  "z": (2, 1),
67  "left": (1, 1),
68  "right": (1, -1),
69  "y": (1, 1),
70  "forward": (0, 1),
71  "backward": (0, -1),
72  "back": (0, -1),
73  "x": (0, 1),
74  }
75 
76  def __init__(self):
77  self._gdict = {}
78  self._group_name = ""
79  self._prev_group_name = ""
80  self._planning_scene_interface = PlanningSceneInterface()
81  self._robot = RobotCommander()
82  self._last_plan = None
83  self._db_host = None
84  self._db_port = 33829
85  self._trace = False
86 
87  def get_active_group(self):
88  if len(self._group_name) > 0:
89  return self._gdict[self._group_name]
90  else:
91  return None
92 
93  def get_loaded_groups(self):
94  return self._gdict.keys()
95 
96  def execute(self, cmd):
97  cmd = self.resolve_command_alias(cmd)
98  result = self.execute_generic_command(cmd)
99  if result != None:
100  return result
101  else:
102  if len(self._group_name) > 0:
103  return self.execute_group_command(self._gdict[self._group_name], cmd)
104  else:
105  return (
106  MoveGroupInfoLevel.INFO,
107  self.get_help()
108  + "\n\nNo groups initialized yet. You must call the 'use' or the 'load' command first.\n",
109  )
110 
111  def execute_generic_command(self, cmd):
112  if os.path.isfile("cmd/" + cmd):
113  cmd = "load cmd/" + cmd
114  cmdlow = cmd.lower()
115  if cmdlow.startswith("use"):
116  if cmdlow == "use":
117  return (MoveGroupInfoLevel.INFO, "\n".join(self.get_loaded_groups()))
118  clist = cmd.split()
119  clist[0] = clist[0].lower()
120  if len(clist) == 2:
121  if clist[0] == "use":
122  if clist[1] == "previous":
123  clist[1] = self._prev_group_name
124  if len(clist[1]) == 0:
125  return (MoveGroupInfoLevel.DEBUG, "OK")
126  if clist[1] in self._gdict:
127  self._prev_group_name = self._group_name
128  self._group_name = clist[1]
129  return (MoveGroupInfoLevel.DEBUG, "OK")
130  else:
131  try:
132  mg = MoveGroupCommander(clist[1])
133  self._gdict[clist[1]] = mg
134  self._group_name = clist[1]
135  return (MoveGroupInfoLevel.DEBUG, "OK")
136  except MoveItCommanderException as e:
137  return (
138  MoveGroupInfoLevel.FAIL,
139  "Initializing " + clist[1] + ": " + str(e),
140  )
141  except:
142  return (
143  MoveGroupInfoLevel.FAIL,
144  "Unable to initialize " + clist[1],
145  )
146  elif cmdlow.startswith("trace"):
147  if cmdlow == "trace":
148  return (
149  MoveGroupInfoLevel.INFO,
150  "trace is on" if self._trace else "trace is off",
151  )
152  clist = cmdlow.split()
153  if clist[0] == "trace" and clist[1] == "on":
154  self._trace = True
155  return (MoveGroupInfoLevel.DEBUG, "OK")
156  if clist[0] == "trace" and clist[1] == "off":
157  self._trace = False
158  return (MoveGroupInfoLevel.DEBUG, "OK")
159  elif cmdlow.startswith("load"):
160  filename = self.DEFAULT_FILENAME
161  clist = cmd.split()
162  if len(clist) > 2:
163  return (MoveGroupInfoLevel.WARN, "Unable to parse load command")
164  if len(clist) == 2:
165  filename = clist[1]
166  if not os.path.isfile(filename):
167  filename = "cmd/" + filename
168  line_num = 0
169  line_content = ""
170  try:
171  f = open(filename, "r")
172  for line in f:
173  line_num += 1
174  line = line.rstrip()
175  line_content = line
176  if self._trace:
177  print("%s:%d: %s" % (filename, line_num, line_content))
178  comment = line.find("#")
179  if comment != -1:
180  line = line[0:comment].rstrip()
181  if line != "":
182  self.execute(line.rstrip())
183  line_content = ""
184  return (MoveGroupInfoLevel.DEBUG, "OK")
185  except MoveItCommanderException as e:
186  if line_num > 0:
187  return (
188  MoveGroupInfoLevel.WARN,
189  "Error at %s:%d: %s\n%s"
190  % (filename, line_num, line_content, str(e)),
191  )
192  else:
193  return (
194  MoveGroupInfoLevel.WARN,
195  "Processing " + filename + ": " + str(e),
196  )
197  except:
198  if line_num > 0:
199  return (
200  MoveGroupInfoLevel.WARN,
201  "Error at %s:%d: %s" % (filename, line_num, line_content),
202  )
203  else:
204  return (MoveGroupInfoLevel.WARN, "Unable to load " + filename)
205  elif cmdlow.startswith("save"):
206  filename = self.DEFAULT_FILENAME
207  clist = cmd.split()
208  if len(clist) > 2:
209  return (MoveGroupInfoLevel.WARN, "Unable to parse save command")
210  if len(clist) == 2:
211  filename = clist[1]
212  try:
213  f = open(filename, "w")
214  for gr in self._gdict.keys():
215  f.write("use " + gr + "\n")
216  known = self._gdict[gr].get_remembered_joint_values()
217  for v in known.keys():
218  f.write(
219  v + " = [" + " ".join([str(x) for x in known[v]]) + "]\n"
220  )
221  if self._db_host != None:
222  f.write(
223  "database " + self._db_host + " " + str(self._db_port) + "\n"
224  )
225  return (MoveGroupInfoLevel.DEBUG, "OK")
226  except:
227  return (MoveGroupInfoLevel.WARN, "Unable to save " + filename)
228  else:
229  return None
230 
231  def execute_group_command(self, g, cmdorig):
232  cmd = cmdorig.lower()
233 
234  if cmd == "stop":
235  g.stop()
236  return (MoveGroupInfoLevel.DEBUG, "OK")
237 
238  if cmd == "id":
239  return (MoveGroupInfoLevel.INFO, g.get_name())
240 
241  if cmd == "help":
242  return (MoveGroupInfoLevel.INFO, self.get_help())
243 
244  if cmd == "vars":
245  known = g.get_remembered_joint_values()
246  return (MoveGroupInfoLevel.INFO, "[" + " ".join(known.keys()) + "]")
247 
248  if cmd == "joints":
249  joints = g.get_joints()
250  return (
251  MoveGroupInfoLevel.INFO,
252  "\n"
253  + "\n".join([str(i) + " = " + joints[i] for i in range(len(joints))])
254  + "\n",
255  )
256 
257  if cmd == "show":
258  return self.command_show(g)
259 
260  if cmd == "current":
261  return self.command_current(g)
262 
263  if cmd == "group_state":
264  return self.command_group_state(g)
265 
266  if cmd == "ground":
267  pose = PoseStamped()
268  pose.pose.position.x = 0
269  pose.pose.position.y = 0
270  # offset such that the box is 0.1 mm below ground (to prevent collision with the robot itself)
271  pose.pose.position.z = -0.0501
272  pose.pose.orientation.x = 0
273  pose.pose.orientation.y = 0
274  pose.pose.orientation.z = 0
275  pose.pose.orientation.w = 1
276  pose.header.stamp = rospy.get_rostime()
277  pose.header.frame_id = self._robot.get_root_link()
278  self._planning_scene_interface.attach_box(
279  self._robot.get_root_link(), "ground", pose, (3, 3, 0.1)
280  )
281  return (MoveGroupInfoLevel.INFO, "Added ground")
282 
283  if cmd == "eef":
284  if len(g.get_end_effector_link()) > 0:
285  return (MoveGroupInfoLevel.INFO, g.get_end_effector_link())
286  else:
287  return (MoveGroupInfoLevel.INFO, "There is no end effector defined")
288 
289  if cmd == "database":
290  if self._db_host == None:
291  return (MoveGroupInfoLevel.INFO, "Not connected to a database")
292  else:
293  return (
294  MoveGroupInfoLevel.INFO,
295  "Connected to " + self._db_host + ":" + str(self._db_port),
296  )
297  if cmd == "plan":
298  if self._last_plan != None:
299  return (MoveGroupInfoLevel.INFO, str(self._last_plan))
300  else:
301  return (MoveGroupInfoLevel.INFO, "No previous plan")
302 
303  if cmd == "constrain":
304  g.clear_path_constraints()
305  return (MoveGroupInfoLevel.SUCCESS, "Cleared path constraints")
306 
307  if cmd == "tol" or cmd == "tolerance":
308  return (
309  MoveGroupInfoLevel.INFO,
310  "Joint = %0.6g, Position = %0.6g, Orientation = %0.6g"
311  % g.get_goal_tolerance(),
312  )
313 
314  if cmd == "time":
315  return (MoveGroupInfoLevel.INFO, str(g.get_planning_time()))
316 
317  if cmd == "execute":
318  if self._last_plan != None:
319  if g.execute(self._last_plan):
320  return (MoveGroupInfoLevel.SUCCESS, "Plan submitted for execution")
321  else:
322  return (
323  MoveGroupInfoLevel.WARN,
324  "Failed to submit plan for execution",
325  )
326  else:
327  return (MoveGroupInfoLevel.WARN, "No motion plan computed")
328 
329  # see if we have assignment between variables
330  assign_match = re.match(r"^(\w+)\s*=\s*(\w+)$", cmd)
331  if assign_match:
332  known = g.get_remembered_joint_values()
333  if assign_match.group(2) in known:
334  g.remember_joint_values(
335  assign_match.group(1), known[assign_match.group(2)]
336  )
337  return (
338  MoveGroupInfoLevel.SUCCESS,
339  assign_match.group(1)
340  + " is now the same as "
341  + assign_match.group(2),
342  )
343  else:
344  return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
345 
346  # see if we have assignment of matlab-like vector syntax
347  set_match = re.match(r"^(\w+)\s*=\s*\[([\d\.e\-\+\s]+)\]$", cmd)
348  if set_match:
349  try:
350  g.remember_joint_values(
351  set_match.group(1), [float(x) for x in set_match.group(2).split()]
352  )
353  return (
354  MoveGroupInfoLevel.SUCCESS,
355  "Remembered joint values ["
356  + set_match.group(2)
357  + "] under the name "
358  + set_match.group(1),
359  )
360  except:
361  return (
362  MoveGroupInfoLevel.WARN,
363  "Unable to parse joint value [" + set_match.group(2) + "]",
364  )
365 
366  # see if we have assignment of matlab-like element update
367  component_match = re.match(
368  r"^(\w+)\s*\[\s*(\d+)\s*\]\s*=\s*([\d\.e\-\+]+)$", cmd
369  )
370  if component_match:
371  known = g.get_remembered_joint_values()
372  if component_match.group(1) in known:
373  try:
374  val = known[component_match.group(1)]
375  val[int(component_match.group(2))] = float(component_match.group(3))
376  g.remember_joint_values(component_match.group(1), val)
377  return (
378  MoveGroupInfoLevel.SUCCESS,
379  "Updated "
380  + component_match.group(1)
381  + "["
382  + component_match.group(2)
383  + "]",
384  )
385  except:
386  return (
387  MoveGroupInfoLevel.WARN,
388  "Unable to parse index or value in '" + cmd + "'",
389  )
390  else:
391  return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
392 
393  clist = cmdorig.split()
394  clist[0] = clist[0].lower()
395 
396  # if this is an unknown one-word command, it is probably a variable
397  if len(clist) == 1:
398  known = g.get_remembered_joint_values()
399  if cmd in known:
400  return (
401  MoveGroupInfoLevel.INFO,
402  "[" + " ".join([str(x) for x in known[cmd]]) + "]",
403  )
404  else:
405  return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
406 
407  # command with one argument
408  if len(clist) == 2:
409  if clist[0] == "go":
410  self._last_plan = None
411  if clist[1] == "rand" or clist[1] == "random":
412  vals = g.get_random_joint_values()
413  g.set_joint_value_target(vals)
414  if g.go():
415  return (
416  MoveGroupInfoLevel.SUCCESS,
417  "Moved to random target ["
418  + " ".join([str(x) for x in vals])
419  + "]",
420  )
421  else:
422  return (
423  MoveGroupInfoLevel.FAIL,
424  "Failed while moving to random target ["
425  + " ".join([str(x) for x in vals])
426  + "]",
427  )
428  else:
429  try:
430  g.set_named_target(clist[1])
431  if g.go():
432  return (MoveGroupInfoLevel.SUCCESS, "Moved to " + clist[1])
433  else:
434  return (
435  MoveGroupInfoLevel.FAIL,
436  "Failed while moving to " + clist[1],
437  )
438  except MoveItCommanderException as e:
439  return (
440  MoveGroupInfoLevel.WARN,
441  "Going to " + clist[1] + ": " + str(e),
442  )
443  except:
444  return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown")
445  if clist[0] == "plan":
446  self._last_plan = None
447  vals = None
448  if clist[1] == "rand" or clist[1] == "random":
449  vals = g.get_random_joint_values()
450  g.set_joint_value_target(vals)
451  self._last_plan = g.plan()[1] # The trajectory msg
452  else:
453  try:
454  g.set_named_target(clist[1])
455  self._last_plan = g.plan()[1] # The trajectory msg
456  except MoveItCommanderException as e:
457  return (
458  MoveGroupInfoLevel.WARN,
459  "Planning to " + clist[1] + ": " + str(e),
460  )
461  except:
462  return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown")
463  if self._last_plan != None:
464  if (
465  len(self._last_plan.joint_trajectory.points) == 0
466  and len(self._last_plan.multi_dof_joint_trajectory.points) == 0
467  ):
468  self._last_plan = None
469  dest_str = clist[1]
470  if vals != None:
471  dest_str = "[" + " ".join([str(x) for x in vals]) + "]"
472  if self._last_plan != None:
473  return (MoveGroupInfoLevel.SUCCESS, "Planned to " + dest_str)
474  else:
475  return (
476  MoveGroupInfoLevel.FAIL,
477  "Failed while planning to " + dest_str,
478  )
479  elif clist[0] == "pick":
480  self._last_plan = None
481  if g.pick(clist[1]):
482  return (MoveGroupInfoLevel.SUCCESS, "Picked object " + clist[1])
483  else:
484  return (
485  MoveGroupInfoLevel.FAIL,
486  "Failed while trying to pick object " + clist[1],
487  )
488  elif clist[0] == "place":
489  self._last_plan = None
490  if g.place(clist[1]):
491  return (MoveGroupInfoLevel.SUCCESS, "Placed object " + clist[1])
492  else:
493  return (
494  MoveGroupInfoLevel.FAIL,
495  "Failed while trying to place object " + clist[1],
496  )
497  elif clist[0] == "planner":
498  g.set_planner_id(clist[1])
499  return (MoveGroupInfoLevel.SUCCESS, "Planner is now " + clist[1])
500  elif clist[0] == "record" or clist[0] == "rec":
501  g.remember_joint_values(clist[1])
502  return (
503  MoveGroupInfoLevel.SUCCESS,
504  "Remembered current joint values under the name " + clist[1],
505  )
506  elif clist[0] == "rand" or clist[0] == "random":
507  g.remember_joint_values(clist[1], g.get_random_joint_values())
508  return (
509  MoveGroupInfoLevel.SUCCESS,
510  "Remembered random joint values under the name " + clist[1],
511  )
512  elif clist[0] == "del" or clist[0] == "delete":
513  g.forget_joint_values(clist[1])
514  return (
515  MoveGroupInfoLevel.SUCCESS,
516  "Forgot joint values under the name " + clist[1],
517  )
518  elif clist[0] == "show":
519  known = g.get_remembered_joint_values()
520  if clist[1] in known:
521  return (
522  MoveGroupInfoLevel.INFO,
523  "[" + " ".join([str(x) for x in known[clist[1]]]) + "]",
524  )
525  else:
526  return (
527  MoveGroupInfoLevel.WARN,
528  "Joint values for " + clist[1] + " are not known",
529  )
530  elif clist[0] == "tol" or clist[0] == "tolerance":
531  try:
532  g.set_goal_tolerance(float(clist[1]))
533  return (MoveGroupInfoLevel.SUCCESS, "OK")
534  except:
535  return (
536  MoveGroupInfoLevel.WARN,
537  "Unable to parse tolerance value '" + clist[1] + "'",
538  )
539  elif clist[0] == "time":
540  try:
541  g.set_planning_time(float(clist[1]))
542  return (MoveGroupInfoLevel.SUCCESS, "OK")
543  except:
544  return (
545  MoveGroupInfoLevel.WARN,
546  "Unable to parse planning duration value '" + clist[1] + "'",
547  )
548  elif clist[0] == "constrain":
549  try:
550  g.set_path_constraints(clist[1])
551  return (MoveGroupInfoLevel.SUCCESS, "OK")
552  except:
553  if self._db_host != None:
554  return (
555  MoveGroupInfoLevel.WARN,
556  "Constraint " + clist[1] + " is not known.",
557  )
558  else:
559  return (MoveGroupInfoLevel.WARN, "Not connected to a database.")
560  elif clist[0] == "wait":
561  try:
562  time.sleep(float(clist[1]))
563  return (MoveGroupInfoLevel.SUCCESS, clist[1] + " seconds passed")
564  except:
565  return (
566  MoveGroupInfoLevel.WARN,
567  "Unable to wait '" + clist[1] + "' seconds",
568  )
569  elif clist[0] == "database":
570  try:
571  g.set_constraints_database(clist[1], self._db_port)
572  self._db_host = clist[1]
573  return (
574  MoveGroupInfoLevel.SUCCESS,
575  "Connected to " + self._db_host + ":" + str(self._db_port),
576  )
577  except:
578  return (
579  MoveGroupInfoLevel.WARN,
580  "Unable to connect to '"
581  + clist[1]
582  + ":"
583  + str(self._db_port)
584  + "'",
585  )
586  else:
587  return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
588 
589  if len(clist) == 3:
590  if clist[0] == "go" and clist[1] in self.GO_DIRS:
591  self._last_plan = None
592  try:
593  offset = float(clist[2])
594  (axis, factor) = self.GO_DIRS[clist[1]]
595  return self.command_go_offset(g, offset, factor, axis, clist[1])
596  except MoveItCommanderException as e:
597  return (
598  MoveGroupInfoLevel.WARN,
599  "Going " + clist[1] + ": " + str(e),
600  )
601  except:
602  return (
603  MoveGroupInfoLevel.WARN,
604  "Unable to parse distance '" + clist[2] + "'",
605  )
606  elif clist[0] == "allow" and (clist[1] == "look" or clist[1] == "looking"):
607  if (
608  clist[2] == "1"
609  or clist[2] == "true"
610  or clist[2] == "T"
611  or clist[2] == "True"
612  ):
613  g.allow_looking(True)
614  else:
615  g.allow_looking(False)
616  return (MoveGroupInfoLevel.DEBUG, "OK")
617  elif clist[0] == "allow" and (
618  clist[1] == "replan" or clist[1] == "replanning"
619  ):
620  if (
621  clist[2] == "1"
622  or clist[2] == "true"
623  or clist[2] == "T"
624  or clist[2] == "True"
625  ):
626  g.allow_replanning(True)
627  else:
628  g.allow_replanning(False)
629  return (MoveGroupInfoLevel.DEBUG, "OK")
630  elif clist[0] == "database":
631  try:
632  g.set_constraints_database(clist[1], int(clist[2]))
633  self._db_host = clist[1]
634  self._db_port = int(clist[2])
635  return (
636  MoveGroupInfoLevel.SUCCESS,
637  "Connected to " + self._db_host + ":" + str(self._db_port),
638  )
639  except:
640  self._db_host = None
641  return (
642  MoveGroupInfoLevel.WARN,
643  "Unable to connect to '" + clist[1] + ":" + clist[2] + "'",
644  )
645  if len(clist) == 4:
646  if clist[0] == "rotate":
647  try:
648  g.set_rpy_target([float(x) for x in clist[1:]])
649  if g.go():
650  return (MoveGroupInfoLevel.SUCCESS, "Rotation complete")
651  else:
652  return (
653  MoveGroupInfoLevel.FAIL,
654  "Failed while rotating to " + " ".join(clist[1:]),
655  )
656  except MoveItCommanderException as e:
657  return (MoveGroupInfoLevel.WARN, str(e))
658  except:
659  return (
660  MoveGroupInfoLevel.WARN,
661  "Unable to parse X-Y-Z rotation values '"
662  + " ".join(clist[1:])
663  + "'",
664  )
665  if len(clist) >= 7:
666  if clist[0] == "go":
667  self._last_plan = None
668  approx = False
669  if len(clist) > 7:
670  if clist[7] == "approx" or clist[7] == "approximate":
671  approx = True
672  try:
673  p = Pose()
674  p.position.x = float(clist[1])
675  p.position.y = float(clist[2])
676  p.position.z = float(clist[3])
677  q = tf.transformations.quaternion_from_euler(
678  float(clist[4]), float(clist[5]), float(clist[6])
679  )
680  p.orientation.x = q[0]
681  p.orientation.y = q[1]
682  p.orientation.z = q[2]
683  p.orientation.w = q[3]
684  if approx:
685  g.set_joint_value_target(p, True)
686  else:
687  g.set_pose_target(p)
688  if g.go():
689  return (
690  MoveGroupInfoLevel.SUCCESS,
691  "Moved to pose target\n%s\n" % (str(p)),
692  )
693  else:
694  return (
695  MoveGroupInfoLevel.FAIL,
696  "Failed while moving to pose \n%s\n" % (str(p)),
697  )
698  except MoveItCommanderException as e:
699  return (
700  MoveGroupInfoLevel.WARN,
701  "Going to pose target: %s" % (str(e)),
702  )
703  except:
704  return (
705  MoveGroupInfoLevel.WARN,
706  "Unable to parse pose '" + " ".join(clist[1:]) + "'",
707  )
708 
709  return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
710 
711  def command_show(self, g):
712  known = g.get_remembered_joint_values()
713  res = []
714  for k in known.keys():
715  res.append(k + " = [" + " ".join([str(x) for x in known[k]]) + "]")
716  return (MoveGroupInfoLevel.INFO, "\n".join(res))
717 
718  def command_group_state(self, g):
719  res = (
720  ' <group_state name="custom_state" group="{}">\n'.format(g.get_name())
721  + "\n".join(
722  [
723  ' <joint name="{}" value="{}"/>'.format(j, v)
724  for j, v in zip(g._g.get_variables(), g.get_current_joint_values())
725  ]
726  )
727  + "\n </group_state>"
728  )
729  return (MoveGroupInfoLevel.INFO, res)
730 
731  def command_current(self, g):
732  res = (
733  "joints = ["
734  + ", ".join([str(x) for x in g.get_current_joint_values()])
735  + "]"
736  )
737  if len(g.get_end_effector_link()) > 0:
738  res = (
739  res
740  + "\n\n"
741  + g.get_end_effector_link()
742  + " pose = [\n"
743  + str(g.get_current_pose())
744  + " ]"
745  )
746  res = (
747  res
748  + "\n"
749  + g.get_end_effector_link()
750  + " RPY = "
751  + str(g.get_current_rpy())
752  )
753  return (MoveGroupInfoLevel.INFO, res)
754 
755  def command_go_offset(self, g, offset, factor, dimension_index, direction_name):
756  if g.has_end_effector_link():
757  try:
758  g.shift_pose_target(dimension_index, factor * offset)
759  if g.go():
760  return (
761  MoveGroupInfoLevel.SUCCESS,
762  "Moved " + direction_name + " by " + str(offset) + " m",
763  )
764  else:
765  return (
766  MoveGroupInfoLevel.FAIL,
767  "Failed while moving " + direction_name,
768  )
769  except MoveItCommanderException as e:
770  return (MoveGroupInfoLevel.WARN, str(e))
771  except:
772  return (MoveGroupInfoLevel.WARN, "Unable to process pose update")
773  else:
774  return (
775  MoveGroupInfoLevel.WARN,
776  "No known end effector. Cannot move " + direction_name,
777  )
778 
779  def resolve_command_alias(self, cmdorig):
780  cmd = cmdorig.lower()
781  if cmd == "which":
782  return "id"
783  if cmd == "groups":
784  return "use"
785  return cmdorig
786 
787  def get_help(self):
788  res = []
789  res.append("Known commands:")
790  res.append(" help show this screen")
791  res.append(" allow looking <true|false> enable/disable looking around")
792  res.append(" allow replanning <true|false> enable/disable replanning")
793  res.append(" constrain clear path constraints")
794  res.append(
795  " constrain <name> use the constraint <name> as a path constraint"
796  )
797  res.append(" current show the current state of the active group")
798  res.append(" group_state current state to copy&paste to srdf file")
799  res.append(
800  " database display the current database connection (if any)"
801  )
802  res.append(
803  " delete <name> forget the joint values under the name <name>"
804  )
805  res.append(
806  " eef print the name of the end effector attached to the current group"
807  )
808  res.append(" execute execute a previously computed motion plan")
809  res.append(
810  " go <name> plan and execute a motion to the state <name>"
811  )
812  res.append(" go rand plan and execute a motion to a random state")
813  res.append(
814  " go <dir> <dx>| plan and execute a motion in direction up|down|left|right|forward|backward for distance <dx>"
815  )
816  res.append(" ground add a ground plane to the planning scene")
817  res.append(
818  " id|which display the name of the group that is operated on"
819  )
820  res.append(
821  " joints display names of the joints in the active group"
822  )
823  res.append(
824  " load [<file>] load a set of interpreted commands from a file"
825  )
826  res.append(" pick <name> pick up object <name>")
827  res.append(" place <name> place object <name>")
828  res.append(" plan <name> plan a motion to the state <name>")
829  res.append(" plan rand plan a motion to a random state")
830  res.append(" planner <name> use planner <name> to plan next motion")
831  res.append(
832  " record <name> record the current joint values under the name <name>"
833  )
834  res.append(
835  " rotate <x> <y> <z> plan and execute a motion to a specified orientation (about the X,Y,Z axes)"
836  )
837  res.append(
838  " save [<file>] save the currently known variables as a set of commands"
839  )
840  res.append(
841  " show display the names and values of the known states"
842  )
843  res.append(" show <name> display the value of a state")
844  res.append(" stop stop the active group")
845  res.append(" time show the configured allowed planning time")
846  res.append(" time <val> set the allowed planning time")
847  res.append(
848  " tolerance show the tolerance for reaching the goal region"
849  )
850  res.append(
851  " tolerance <val> set the tolerance for reaching the goal region"
852  )
853  res.append(" trace <on|off> enable/disable replanning or looking around")
854  res.append(
855  " use <name> switch to using the group named <name> (and load it if necessary)"
856  )
857  res.append(" use|groups show the group names that are already loaded")
858  res.append(" vars display the names of the known states")
859  res.append(" wait <dt> sleep for <dt> seconds")
860  res.append(" x = y assign the value of y to x")
861  res.append(" x = [v1 v2...] assign a vector of values to x")
862  res.append(" x[idx] = val assign a value to dimension idx of x")
863  return "\n".join(res)
864 
865  def get_keywords(self):
866  known_vars = []
867  known_constr = []
868  if self.get_active_group() != None:
869  known_vars = self.get_active_group().get_remembered_joint_values().keys()
870  known_constr = self.get_active_group().get_known_constraints()
871  groups = self._robot.get_group_names()
872  return {
873  "go": ["up", "down", "left", "right", "backward", "forward", "random"]
874  + list(known_vars),
875  "help": [],
876  "record": known_vars,
877  "show": known_vars,
878  "wait": [],
879  "delete": known_vars,
880  "database": [],
881  "group_state": [],
882  "current": [],
883  "use": groups,
884  "load": [],
885  "save": [],
886  "pick": [],
887  "place": [],
888  "plan": known_vars,
889  "allow": ["replanning", "looking"],
890  "constrain": known_constr,
891  "vars": [],
892  "joints": [],
893  "tolerance": [],
894  "time": [],
895  "eef": [],
896  "execute": [],
897  "ground": [],
898  "id": [],
899  }
moveit_commander.interpreter.MoveGroupCommandInterpreter._trace
_trace
Definition: interpreter.py:85
moveit_commander.move_group.MoveGroupCommander
Definition: move_group.py:57
moveit_commander.interpreter.MoveGroupCommandInterpreter
Definition: interpreter.py:57
moveit_commander.interpreter.MoveGroupCommandInterpreter.__init__
def __init__(self)
Definition: interpreter.py:76
moveit_commander.interpreter.MoveGroupCommandInterpreter._last_plan
_last_plan
Definition: interpreter.py:82
moveit_commander.interpreter.MoveGroupCommandInterpreter._prev_group_name
_prev_group_name
Definition: interpreter.py:79
moveit_commander.interpreter.MoveGroupCommandInterpreter.resolve_command_alias
def resolve_command_alias(self, cmdorig)
Definition: interpreter.py:779
moveit_commander.interpreter.MoveGroupCommandInterpreter.GO_DIRS
dictionary GO_DIRS
Definition: interpreter.py:63
moveit_commander.interpreter.MoveGroupCommandInterpreter.get_active_group
def get_active_group(self)
Definition: interpreter.py:87
moveit_commander.interpreter.MoveGroupCommandInterpreter.command_current
def command_current(self, g)
Definition: interpreter.py:731
moveit_commander.interpreter.MoveGroupCommandInterpreter.DEFAULT_FILENAME
string DEFAULT_FILENAME
Definition: interpreter.py:62
moveit_commander.interpreter.MoveGroupCommandInterpreter.get_loaded_groups
def get_loaded_groups(self)
Definition: interpreter.py:93
moveit_commander.interpreter.MoveGroupCommandInterpreter.execute
def execute(self, cmd)
Definition: interpreter.py:96
moveit_commander.interpreter.MoveGroupCommandInterpreter.execute_generic_command
def execute_generic_command(self, cmd)
Definition: interpreter.py:111
moveit_commander.interpreter.MoveGroupCommandInterpreter._robot
_robot
Definition: interpreter.py:81
moveit_commander.interpreter.MoveGroupCommandInterpreter._planning_scene_interface
_planning_scene_interface
Definition: interpreter.py:80
moveit_commander.interpreter.MoveGroupCommandInterpreter._db_host
_db_host
Definition: interpreter.py:83
moveit_commander.interpreter.MoveGroupInfoLevel
Definition: interpreter.py:49
moveit_commander.interpreter.MoveGroupCommandInterpreter.get_help
def get_help(self)
Definition: interpreter.py:787
moveit_commander.interpreter.MoveGroupCommandInterpreter.execute_group_command
def execute_group_command(self, g, cmdorig)
Definition: interpreter.py:231
moveit_commander.interpreter.MoveGroupCommandInterpreter._db_port
_db_port
Definition: interpreter.py:84
moveit_commander.interpreter.MoveGroupCommandInterpreter._group_name
_group_name
Definition: interpreter.py:78
moveit_commander.interpreter.MoveGroupCommandInterpreter._gdict
_gdict
Definition: interpreter.py:77
moveit_commander.interpreter.MoveGroupCommandInterpreter.command_group_state
def command_group_state(self, g)
Definition: interpreter.py:718
moveit_commander.robot.RobotCommander
Definition: robot.py:43
moveit_commander.interpreter.MoveGroupCommandInterpreter.command_go_offset
def command_go_offset(self, g, offset, factor, dimension_index, direction_name)
Definition: interpreter.py:755
moveit_commander.interpreter.MoveGroupCommandInterpreter.command_show
def command_show(self, g)
Definition: interpreter.py:711
moveit_commander.planning_scene_interface.PlanningSceneInterface
Definition: planning_scene_interface.py:60
moveit_commander.interpreter.MoveGroupCommandInterpreter.get_keywords
def get_keywords(self)
Definition: interpreter.py:865

moveit_commander
Author(s): Ioan Sucan
autogenerated on Fri May 3 2024 02:30:26