interpreter.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2012, Willow Garage, Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions
00008 # are met:
00009 #
00010 #  * Redistributions of source code must retain the above copyright
00011 #    notice, this list of conditions and the following disclaimer.
00012 #  * Redistributions in binary form must reproduce the above
00013 #    copyright notice, this list of conditions and the following
00014 #    disclaimer in the documentation and/or other materials provided
00015 #    with the distribution.
00016 #  * Neither the name of Willow Garage, Inc. nor the names of its
00017 #    contributors may be used to endorse or promote products derived
00018 #    from this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 # POSSIBILITY OF SUCH DAMAGE.
00032 #
00033 # Author: Ioan Sucan
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             # offset such that the box is 0.1 mm below ground (to prevent collision with the robot itself)
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         # see if we have assignment between variables
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         # see if we have assignment of matlab-like vector syntax
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         # see if we have assignment of matlab-like element update
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         # if this is an unknown one-word command, it is probably a variable
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         # command with one argument
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':[]}


moveit_commander
Author(s): Ioan Sucan
autogenerated on Wed Jun 19 2019 19:24:56