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 import rospy
00036 from moveit_commander import RobotCommander, MoveGroupCommander, PlanningSceneInterface, MoveItCommanderException
00037 from geometry_msgs.msg import Pose, PoseStamped
00038 import tf
00039 import re
00040 import time
00041 import os.path
00042
00043 class MoveGroupInfoLevel:
00044 FAIL = 1
00045 WARN = 2
00046 SUCCESS = 3
00047 INFO = 4
00048 DEBUG = 5
00049
00050 class MoveGroupCommandInterpreter(object):
00051 """
00052 Interpreter for simple commands
00053 """
00054
00055 DEFAULT_FILENAME = "move_group.cfg"
00056 GO_DIRS = {"up" : (2,1), "down" : (2, -1), "z" : (2, 1),
00057 "left" : (1, 1), "right" : (1, -1), "y" : (1, 1),
00058 "forward" : (0, 1), "backward" : (0, -1), "back" : (0, -1), "x" : (0, 1) }
00059
00060 def __init__(self):
00061 self._gdict = {}
00062 self._group_name = ""
00063 self._prev_group_name = ""
00064 self._planning_scene_interface = PlanningSceneInterface()
00065 self._robot = RobotCommander()
00066 self._last_plan = None
00067 self._db_host = None
00068 self._db_port = 33829
00069 self._trace = False
00070
00071 def get_active_group(self):
00072 if len(self._group_name) > 0:
00073 return self._gdict[self._group_name]
00074 else:
00075 return None
00076
00077 def get_loaded_groups(self):
00078 return self._gdict.keys()
00079
00080 def execute(self, cmd):
00081 cmd = self.resolve_command_alias(cmd)
00082 result = self.execute_generic_command(cmd)
00083 if result != None:
00084 return result
00085 else:
00086 if len(self._group_name) > 0:
00087 return self.execute_group_command(self._gdict[self._group_name], cmd)
00088 else:
00089 return (MoveGroupInfoLevel.INFO, self.get_help() + "\n\nNo groups initialized yet. You must call the 'use' or the 'load' command first.\n")
00090
00091 def execute_generic_command(self, cmd):
00092 if os.path.isfile("cmd/" + cmd):
00093 cmd = "load cmd/" + cmd
00094 cmdlow = cmd.lower()
00095 if cmdlow.startswith("use"):
00096 if cmdlow == "use":
00097 return (MoveGroupInfoLevel.INFO, "\n".join(self.get_loaded_groups()))
00098 clist = cmd.split()
00099 clist[0] = clist[0].lower()
00100 if len(clist) == 2:
00101 if clist[0] == "use":
00102 if clist[1] == "previous":
00103 clist[1] = self._prev_group_name
00104 if len(clist[1]) == 0:
00105 return (MoveGroupInfoLevel.DEBUG, "OK")
00106 if self._gdict.has_key(clist[1]):
00107 self._prev_group_name = self._group_name
00108 self._group_name = clist[1]
00109 return (MoveGroupInfoLevel.DEBUG, "OK")
00110 else:
00111 try:
00112 mg = MoveGroupCommander(clist[1])
00113 self._gdict[clist[1]] = mg
00114 self._group_name = clist[1]
00115 return (MoveGroupInfoLevel.DEBUG, "OK")
00116 except MoveItCommanderException as e:
00117 return (MoveGroupInfoLevel.FAIL, "Initializing " + clist[1] + ": " + str(e))
00118 except:
00119 return (MoveGroupInfoLevel.FAIL, "Unable to initialize " + clist[1])
00120 elif cmdlow.startswith("trace"):
00121 if cmdlow == "trace":
00122 return (MoveGroupInfoLevel.INFO, "trace is on" if self._trace else "trace is off")
00123 clist = cmdlow.split()
00124 if clist[0] == "trace" and clist[1] == "on":
00125 self._trace = True
00126 return (MoveGroupInfoLevel.DEBUG, "OK")
00127 if clist[0] == "trace" and clist[1] == "off":
00128 self._trace = False
00129 return (MoveGroupInfoLevel.DEBUG, "OK")
00130 elif cmdlow.startswith("load"):
00131 filename = self.DEFAULT_FILENAME
00132 clist = cmd.split()
00133 if len(clist) > 2:
00134 return (MoveGroupInfoLevel.WARN, "Unable to parse load command")
00135 if len(clist) == 2:
00136 filename = clist[1]
00137 if not os.path.isfile(filename):
00138 filename = "cmd/" + filename
00139 line_num = 0
00140 line_content = ""
00141 try:
00142 f = open(filename, 'r')
00143 for line in f:
00144 line_num += 1
00145 line = line.rstrip()
00146 line_content = line
00147 if self._trace:
00148 print ("%s:%d: %s" % (filename, line_num, line_content))
00149 comment = line.find("#")
00150 if comment != -1:
00151 line = line[0:comment].rstrip()
00152 if line != "":
00153 self.execute(line.rstrip())
00154 line_content = ""
00155 return (MoveGroupInfoLevel.DEBUG, "OK")
00156 except MoveItCommanderException as e:
00157 if line_num > 0:
00158 return (MoveGroupInfoLevel.WARN, "Error at %s:%d: %s\n%s" % (filename, line_num, line_content, str(e)))
00159 else:
00160 return (MoveGroupInfoLevel.WARN, "Processing " + filename + ": " + str(e))
00161 except:
00162 if line_num > 0:
00163 return (MoveGroupInfoLevel.WARN, "Error at %s:%d: %s" % (filename, line_num, line_content))
00164 else:
00165 return (MoveGroupInfoLevel.WARN, "Unable to load " + filename)
00166 elif cmdlow.startswith("save"):
00167 filename = self.DEFAULT_FILENAME
00168 clist = cmd.split()
00169 if len(clist) > 2:
00170 return (MoveGroupInfoLevel.WARN, "Unable to parse save command")
00171 if len(clist) == 2:
00172 filename = clist[1]
00173 try:
00174 f = open(filename, 'w')
00175 for gr in self._gdict.keys():
00176 f.write("use " + gr + "\n")
00177 known = self._gdict[gr].get_remembered_joint_values()
00178 for v in known.keys():
00179 f.write(v + " = [" + " ".join([str(x) for x in known[v]]) + "]\n")
00180 if self._db_host != None:
00181 f.write("database " + self._db_host + " " + str(self._db_port) + "\n")
00182 return (MoveGroupInfoLevel.DEBUG, "OK")
00183 except:
00184 return (MoveGroupInfoLevel.WARN, "Unable to save " + filename)
00185 else:
00186 return None
00187
00188 def execute_group_command(self, g, cmdorig):
00189 cmd = cmdorig.lower()
00190
00191 if cmd == "stop":
00192 g.stop()
00193 return (MoveGroupInfoLevel.DEBUG, "OK")
00194
00195 if cmd == "id":
00196 return (MoveGroupInfoLevel.INFO, g.get_name())
00197
00198 if cmd == "help":
00199 return (MoveGroupInfoLevel.INFO, self.get_help())
00200
00201 if cmd == "vars":
00202 known = g.get_remembered_joint_values()
00203 return (MoveGroupInfoLevel.INFO, "[" + " ".join(known.keys()) + "]")
00204
00205 if cmd == "joints":
00206 joints = g.get_joints()
00207 return (MoveGroupInfoLevel.INFO, "\n" + "\n".join([str(i) + " = " + joints[i] for i in range(len(joints))]) + "\n")
00208
00209 if cmd == "show":
00210 return self.command_show(g)
00211
00212 if cmd == "current":
00213 return self.command_current(g)
00214
00215 if cmd == "ground":
00216 pose = PoseStamped()
00217 pose.pose.position.x = 0
00218 pose.pose.position.y = 0
00219
00220 pose.pose.position.z = -0.0501
00221 pose.pose.orientation.x = 0
00222 pose.pose.orientation.y = 0
00223 pose.pose.orientation.z = 0
00224 pose.pose.orientation.w = 1
00225 pose.header.stamp = rospy.get_rostime()
00226 pose.header.frame_id = self._robot.get_root_link()
00227 self._planning_scene_interface.attach_box(self._robot.get_root_link(), "ground", pose, (3, 3, 0.1))
00228 return (MoveGroupInfoLevel.INFO, "Added ground")
00229
00230 if cmd == "eef":
00231 if len(g.get_end_effector_link()) > 0:
00232 return (MoveGroupInfoLevel.INFO, g.get_end_effector_link())
00233 else:
00234 return (MoveGroupInfoLevel.INFO, "There is no end effector defined")
00235
00236 if cmd == "database":
00237 if self._db_host == None:
00238 return (MoveGroupInfoLevel.INFO, "Not connected to a database")
00239 else:
00240 return (MoveGroupInfoLevel.INFO, "Connected to " + self._db_host + ":" + str(self._db_port))
00241 if cmd == "plan":
00242 if self._last_plan != None:
00243 return (MoveGroupInfoLevel.INFO, str(self._last_plan))
00244 else:
00245 return (MoveGroupInfoLevel.INFO, "No previous plan")
00246
00247 if cmd == "constrain":
00248 g.clear_path_constraints()
00249 return (MoveGroupInfoLevel.SUCCESS, "Cleared path constraints")
00250
00251 if cmd == "tol" or cmd == "tolerance":
00252 return (MoveGroupInfoLevel.INFO, "Joint = %0.6g, Position = %0.6g, Orientation = %0.6g" % g.get_goal_tolerance())
00253
00254 if cmd == "time":
00255 return (MoveGroupInfoLevel.INFO, str(g.get_planning_time()))
00256
00257 if cmd == "execute":
00258 if self._last_plan != None:
00259 if g.execute(self._last_plan):
00260 return (MoveGroupInfoLevel.SUCCESS, "Plan submitted for execution")
00261 else:
00262 return (MoveGroupInfoLevel.WARN, "Failed to submit plan for execution")
00263 else:
00264 return (MoveGroupInfoLevel.WARN, "No motion plan computed")
00265
00266
00267 assign_match = re.match(r"^(\w+)\s*=\s*(\w+)$", cmd)
00268 if assign_match:
00269 known = g.get_remembered_joint_values()
00270 if known.has_key(assign_match.group(2)):
00271 g.remember_joint_values(assign_match.group(1), known[assign_match.group(2)])
00272 return (MoveGroupInfoLevel.SUCCESS, assign_match.group(1) + " is now the same as " + assign_match.group(2))
00273 else:
00274 return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
00275
00276
00277 set_match = re.match(r"^(\w+)\s*=\s*\[([\d\.e\-\+\s]+)\]$", cmd)
00278 if set_match:
00279 try:
00280 g.remember_joint_values(set_match.group(1), [float(x) for x in set_match.group(2).split()])
00281 return (MoveGroupInfoLevel.SUCCESS, "Remembered joint values [" + set_match.group(2) + "] under the name " + set_match.group(1))
00282 except:
00283 return (MoveGroupInfoLevel.WARN, "Unable to parse joint value [" + set_match.group(2) + "]")
00284
00285
00286 component_match = re.match(r"^(\w+)\s*\[\s*(\d+)\s*\]\s*=\s*([\d\.e\-\+]+)$", cmd)
00287 if component_match:
00288 known = g.get_remembered_joint_values()
00289 if known.has_key(component_match.group(1)):
00290 try:
00291 val = known[component_match.group(1)]
00292 val[int(component_match.group(2))] = float(component_match.group(3))
00293 g.remember_joint_values(component_match.group(1), val)
00294 return (MoveGroupInfoLevel.SUCCESS, "Updated " + component_match.group(1) + "[" + component_match.group(2) + "]")
00295 except:
00296 return (MoveGroupInfoLevel.WARN, "Unable to parse index or value in '" + cmd +"'")
00297 else:
00298 return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
00299
00300 clist = cmdorig.split()
00301 clist[0] = clist[0].lower()
00302
00303
00304 if len(clist) == 1:
00305 known = g.get_remembered_joint_values()
00306 if known.has_key(cmd):
00307 return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[cmd]]) + "]")
00308 else:
00309 return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
00310
00311
00312 if len(clist) == 2:
00313 if clist[0] == "go":
00314 self._last_plan = None
00315 if clist[1] == "rand" or clist[1] == "random":
00316 vals = g.get_random_joint_values()
00317 g.set_joint_value_target(vals)
00318 if g.go():
00319 return (MoveGroupInfoLevel.SUCCESS, "Moved to random target [" + " ".join([str(x) for x in vals]) + "]")
00320 else:
00321 return (MoveGroupInfoLevel.FAIL, "Failed while moving to random target [" + " ".join([str(x) for x in vals]) + "]")
00322 else:
00323 try:
00324 g.set_named_target(clist[1])
00325 if g.go():
00326 return (MoveGroupInfoLevel.SUCCESS, "Moved to " + clist[1])
00327 else:
00328 return (MoveGroupInfoLevel.FAIL, "Failed while moving to " + clist[1])
00329 except MoveItCommanderException as e:
00330 return (MoveGroupInfoLevel.WARN, "Going to " + clist[1] + ": " + str(e))
00331 except:
00332 return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown")
00333 if clist[0] == "plan":
00334 self._last_plan = None
00335 vals = None
00336 if clist[1] == "rand" or clist[1] == "random":
00337 vals = g.get_random_joint_values()
00338 g.set_joint_value_target(vals)
00339 self._last_plan = g.plan()
00340 else:
00341 try:
00342 g.set_named_target(clist[1])
00343 self._last_plan = g.plan()
00344 except MoveItCommanderException as e:
00345 return (MoveGroupInfoLevel.WARN, "Planning to " + clist[1] + ": " + str(e))
00346 except:
00347 return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown")
00348 if self._last_plan != None:
00349 if len(self._last_plan.joint_trajectory.points) == 0 and len(self._last_plan.multi_dof_joint_trajectory.points) == 0:
00350 self._last_plan = None
00351 dest_str = clist[1]
00352 if vals != None:
00353 dest_str = "[" + " ".join([str(x) for x in vals]) + "]"
00354 if self._last_plan != None:
00355 return (MoveGroupInfoLevel.SUCCESS, "Planned to " + dest_str)
00356 else:
00357 return (MoveGroupInfoLevel.FAIL, "Failed while planning to " + dest_str)
00358 elif clist[0] == "pick":
00359 self._last_plan = None
00360 if g.pick(clist[1]):
00361 return (MoveGroupInfoLevel.SUCCESS, "Picked object " + clist[1])
00362 else:
00363 return (MoveGroupInfoLevel.FAIL, "Failed while trying to pick object " + clist[1])
00364 elif clist[0] == "place":
00365 self._last_plan = None
00366 if g.place(clist[1]):
00367 return (MoveGroupInfoLevel.SUCCESS, "Placed object " + clist[1])
00368 else:
00369 return (MoveGroupInfoLevel.FAIL, "Failed while trying to place object " + clist[1])
00370 elif clist[0] == "planner":
00371 g.set_planner_id(clist[1])
00372 return (MoveGroupInfoLevel.SUCCESS, "Planner is now " + clist[1])
00373 elif clist[0] == "record" or clist[0] == "rec":
00374 g.remember_joint_values(clist[1])
00375 return (MoveGroupInfoLevel.SUCCESS, "Remembered current joint values under the name " + clist[1])
00376 elif clist[0] == "rand" or clist[0] == "random":
00377 g.remember_joint_values(clist[1], g.get_random_joint_values())
00378 return (MoveGroupInfoLevel.SUCCESS, "Remembered random joint values under the name " + clist[1])
00379 elif clist[0] == "del" or clist[0] == "delete":
00380 g.forget_joint_values(clist[1])
00381 return (MoveGroupInfoLevel.SUCCESS, "Forgot joint values under the name " + clist[1])
00382 elif clist[0] == "show":
00383 known = g.get_remembered_joint_values()
00384 if known.has_key(clist[1]):
00385 return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[clist[1]]]) + "]")
00386 else:
00387 return (MoveGroupInfoLevel.WARN, "Joint values for " + clist[1] + " are not known")
00388 elif clist[0] == "tol" or clist[0] == "tolerance":
00389 try:
00390 g.set_goal_tolerance(float(clist[1]))
00391 return (MoveGroupInfoLevel.SUCCESS, "OK")
00392 except:
00393 return (MoveGroupInfoLevel.WARN, "Unable to parse tolerance value '" + clist[1] + "'")
00394 elif clist[0] == "time":
00395 try:
00396 g.set_planning_time(float(clist[1]))
00397 return (MoveGroupInfoLevel.SUCCESS, "OK")
00398 except:
00399 return (MoveGroupInfoLevel.WARN, "Unable to parse planning duration value '" + clist[1] + "'")
00400 elif clist[0] == "constrain":
00401 try:
00402 g.set_path_constraints(clist[1])
00403 return (MoveGroupInfoLevel.SUCCESS, "OK")
00404 except:
00405 if self._db_host != None:
00406 return (MoveGroupInfoLevel.WARN, "Constraint " + clist[1] + " is not known.")
00407 else:
00408 return (MoveGroupInfoLevel.WARN, "Not connected to a database.")
00409 elif clist[0] == "wait":
00410 try:
00411 time.sleep(float(clist[1]))
00412 return (MoveGroupInfoLevel.SUCCESS, clist[1] + " seconds passed")
00413 except:
00414 return (MoveGroupInfoLevel.WARN, "Unable to wait '" + clist[1] + "' seconds")
00415 elif clist[0] == "database":
00416 try:
00417 g.set_constraints_database(clist[1], self._db_port)
00418 self._db_host = clist[1]
00419 return (MoveGroupInfoLevel.SUCCESS, "Connected to " + self._db_host + ":" + str(self._db_port))
00420 except:
00421 return (MoveGroupInfoLevel.WARN, "Unable to connect to '" + clist[1] + ":" + str(self._db_port) + "'")
00422 else:
00423 return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
00424
00425 if len(clist) == 3:
00426 if clist[0] == "go" and self.GO_DIRS.has_key(clist[1]):
00427 self._last_plan = None
00428 try:
00429 offset = float(clist[2])
00430 (axis, factor) = self.GO_DIRS[clist[1]]
00431 return self.command_go_offset(g, offset, factor, axis, clist[1])
00432 except MoveItCommanderException as e:
00433 return (MoveGroupInfoLevel.WARN, "Going " + clist[1] + ": " + str(e))
00434 except:
00435 return (MoveGroupInfoLevel.WARN, "Unable to parse distance '" + clist[2] + "'")
00436 elif clist[0] == "allow" and (clist[1] == "look" or clist[1] == "looking"):
00437 if clist[2] == "1" or clist[2] == "true" or clist[2] == "T" or clist[2] == "True":
00438 g.allow_looking(True)
00439 else:
00440 g.allow_looking(False)
00441 return (MoveGroupInfoLevel.DEBUG, "OK")
00442 elif clist[0] == "allow" and (clist[1] == "replan" or clist[1] == "replanning"):
00443 if clist[2] == "1" or clist[2] == "true" or clist[2] == "T" or clist[2] == "True":
00444 g.allow_replanning(True)
00445 else:
00446 g.allow_replanning(False)
00447 return (MoveGroupInfoLevel.DEBUG, "OK")
00448 elif clist[0] == "database":
00449 try:
00450 g.set_constraints_database(clist[1], int(clist[2]))
00451 self._db_host = clist[1]
00452 self._db_port = int(clist[2])
00453 return (MoveGroupInfoLevel.SUCCESS, "Connected to " + self._db_host + ":" + str(self._db_port))
00454 except:
00455 self._db_host = None
00456 return (MoveGroupInfoLevel.WARN, "Unable to connect to '" + clist[1] + ":" + clist[2] + "'")
00457 if len(clist) == 4:
00458 if clist[0] == "rotate":
00459 try:
00460 g.set_rpy_target([float(x) for x in clist[1:]])
00461 if g.go():
00462 return (MoveGroupInfoLevel.SUCCESS, "Rotation complete")
00463 else:
00464 return (MoveGroupInfoLevel.FAIL, "Failed while rotating to " + " ".join(clist[1:]))
00465 except MoveItCommanderException as e:
00466 return (MoveGroupInfoLevel.WARN, str(e))
00467 except:
00468 return (MoveGroupInfoLevel.WARN, "Unable to parse X-Y-Z rotation values '" + " ".join(clist[1:]) + "'")
00469 if len(clist) >= 7:
00470 if clist[0] == "go":
00471 self._last_plan = None
00472 approx = False
00473 if (len(clist) > 7):
00474 if (clist[7] == "approx" or clist[7] == "approximate"):
00475 approx = True
00476 try:
00477 p = Pose()
00478 p.position.x = float(clist[1])
00479 p.position.y = float(clist[2])
00480 p.position.z = float(clist[3])
00481 q = tf.transformations.quaternion_from_euler(float(clist[4]), float(clist[5]), float(clist[6]))
00482 p.orientation.x = q[0]
00483 p.orientation.y = q[1]
00484 p.orientation.z = q[2]
00485 p.orientation.w = q[3]
00486 if approx:
00487 g.set_joint_value_target(p, True)
00488 else:
00489 g.set_pose_target(p)
00490 if g.go():
00491 return (MoveGroupInfoLevel.SUCCESS, "Moved to pose target\n%s\n" % (str(p)))
00492 else:
00493 return (MoveGroupInfoLevel.FAIL, "Failed while moving to pose \n%s\n" % (str(p)))
00494 except MoveItCommanderException as e:
00495 return (MoveGroupInfoLevel.WARN, "Going to pose target: %s" % (str(e)))
00496 except:
00497 return (MoveGroupInfoLevel.WARN, "Unable to parse pose '" + " ".join(clist[1:]) + "'")
00498
00499 return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
00500
00501 def command_show(self, g):
00502 known = g.get_remembered_joint_values()
00503 res = []
00504 for k in known.keys():
00505 res.append(k + " = [" + " ".join([str(x) for x in known[k]]) + "]")
00506 return (MoveGroupInfoLevel.INFO, "\n".join(res))
00507
00508 def command_current(self, g):
00509 res = "joints = [" + " ".join([str(x) for x in g.get_current_joint_values()]) + "]"
00510 if len(g.get_end_effector_link()) > 0:
00511 res = res + "\n" + g.get_end_effector_link() + " pose = [\n" + str(g.get_current_pose()) + " ]"
00512 res = res + "\n" + g.get_end_effector_link() + " RPY = " + str(g.get_current_rpy())
00513 return (MoveGroupInfoLevel.INFO, res)
00514
00515 def command_go_offset(self, g, offset, factor, dimension_index, direction_name):
00516 if g.has_end_effector_link():
00517 try:
00518 g.shift_pose_target(dimension_index, factor * offset)
00519 if g.go():
00520 return (MoveGroupInfoLevel.SUCCESS, "Moved " + direction_name + " by " + str(offset) + " m")
00521 else:
00522 return (MoveGroupInfoLevel.FAIL, "Failed while moving " + direction_name)
00523 except MoveItCommanderException as e:
00524 return (MoveGroupInfoLevel.WARN, str(e))
00525 except:
00526 return (MoveGroupInfoLevel.WARN, "Unable to process pose update")
00527 else:
00528 return (MoveGroupInfoLevel.WARN, "No known end effector. Cannot move " + direction_name)
00529
00530 def resolve_command_alias(self, cmdorig):
00531 cmd = cmdorig.lower()
00532 if cmd == "which":
00533 return "id"
00534 if cmd == "groups":
00535 return "use"
00536 return cmdorig
00537
00538 def get_help(self):
00539 res = []
00540 res.append("Known commands:")
00541 res.append(" help show this screen")
00542 res.append(" allow looking <true|false> enable/disable looking around")
00543 res.append(" allow replanning <true|false> enable/disable replanning")
00544 res.append(" constrain clear path constraints")
00545 res.append(" constrain <name> use the constraint <name> as a path constraint")
00546 res.append(" current show the current state of the active group")
00547 res.append(" database display the current database connection (if any)")
00548 res.append(" delete <name> forget the joint values under the name <name>")
00549 res.append(" eef print the name of the end effector attached to the current group")
00550 res.append(" execute execute a previously computed motion plan")
00551 res.append(" go <name> plan and execute a motion to the state <name>")
00552 res.append(" go rand plan and execute a motion to a random state")
00553 res.append(" go <dir> <dx>| plan and execute a motion in direction up|down|left|right|forward|backward for distance <dx>")
00554 res.append(" ground add a ground plane to the planning scene")
00555 res.append(" id|which display the name of the group that is operated on")
00556 res.append(" joints display names of the joints in the active group")
00557 res.append(" load [<file>] load a set of interpreted commands from a file")
00558 res.append(" pick <name> pick up object <name>")
00559 res.append(" place <name> place object <name>")
00560 res.append(" plan <name> plan a motion to the state <name>")
00561 res.append(" plan rand plan a motion to a random state")
00562 res.append(" planner <name> use planner <name> to plan next motion")
00563 res.append(" record <name> record the current joint values under the name <name>")
00564 res.append(" rotate <x> <y> <z> plan and execute a motion to a specified orientation (about the X,Y,Z axes)")
00565 res.append(" save [<file>] save the currently known variables as a set of commands")
00566 res.append(" show display the names and values of the known states")
00567 res.append(" show <name> display the value of a state")
00568 res.append(" stop stop the active group")
00569 res.append(" time show the configured allowed planning time")
00570 res.append(" time <val> set the allowed planning time")
00571 res.append(" tolerance show the tolerance for reaching the goal region")
00572 res.append(" tolerance <val> set the tolerance for reaching the goal region")
00573 res.append(" trace <on|off> enable/disable replanning or looking around")
00574 res.append(" use <name> switch to using the group named <name> (and load it if necessary)")
00575 res.append(" use|groups show the group names that are already loaded")
00576 res.append(" vars display the names of the known states")
00577 res.append(" wait <dt> sleep for <dt> seconds")
00578 res.append(" x = y assign the value of y to x")
00579 res.append(" x = [v1 v2...] assign a vector of values to x")
00580 res.append(" x[idx] = val assign a value to dimension idx of x")
00581 return "\n".join(res)
00582
00583 def get_keywords(self):
00584 known_vars = []
00585 known_constr = []
00586 if self.get_active_group() != None:
00587 known_vars = self.get_active_group().get_remembered_joint_values().keys()
00588 known_constr = self.get_active_group().get_known_constraints()
00589 groups = self._robot.get_group_names()
00590 return {'go':['up', 'down', 'left', 'right', 'backward', 'forward', 'random'] + known_vars,
00591 'help':[],
00592 'record':known_vars,
00593 'show':known_vars,
00594 'wait':[],
00595 'delete':known_vars,
00596 'database': [],
00597 'current':[],
00598 'use':groups,
00599 'load':[],
00600 'save':[],
00601 'pick':[],
00602 'place':[],
00603 'plan':known_vars,
00604 'allow':['replanning', 'looking'],
00605 'constrain':known_constr,
00606 'vars':[],
00607 'joints':[],
00608 'tolerance':[],
00609 'time':[],
00610 'eef':[],
00611 'execute':[],
00612 'ground':[],
00613 'id':[]}