def get_group_for_joint(joint_names, robot=None, smallest=False, groups=None):
"""Given list of joints find the most appropriate group.
INPUT
link_ids [List of Strings]
robot [RobotCommander]
smallest [Bool]
OUTPUT
group [String]
"""
if not robot:
robot = RobotCommander()
if not groups:
groups = robot.get_group_names()
group = None
n = np.Inf if smallest else 0
for g in groups:
names = robot.get_joint_names(g)
if all_in(joint_names, names):
if smallest and len(names) < n:
n = len(names)
group = g
elif len(names) > n:
n = len(names)
group = g
return group
def init_node(node_name="commander_example"):
'''
Initializing QtApplication and ROS node.
Displaying names of Move Groups defined in the robot.
@type node_name : str
@param node_name : name for ROS node to initialize
'''
global qtapp
qtapp = QApplication(sys.argv)
rospy.init_node(node_name, anonymous=True)
robot = RobotCommander()
group_names = robot.get_group_names()
gnames_info = "Move Groups defined in the robot :"
for name in group_names:
gnames_info = gnames_info + '\n' + name
rospy.loginfo(gnames_info)
class MoveGroupCommandInterpreter(object):
"""
Interpreter for simple commands
"""
DEFAULT_FILENAME = "move_group.cfg"
GO_DIRS = {
"up": (2, 1),
"down": (2, -1),
"z": (2, 1),
"left": (1, 1),
"right": (1, -1),
"y": (1, 1),
"forward": (0, 1),
"backward": (0, -1),
"back": (0, -1),
"x": (0, 1),
}
def __init__(self):
self._gdict = {}
self._group_name = ""
self._prev_group_name = ""
self._planning_scene_interface = PlanningSceneInterface()
self._robot = RobotCommander()
self._last_plan = None
self._db_host = None
self._db_port = 33829
self._trace = False
def get_active_group(self):
if len(self._group_name) > 0:
return self._gdict[self._group_name]
else:
return None
def get_loaded_groups(self):
return self._gdict.keys()
def execute(self, cmd):
cmd = self.resolve_command_alias(cmd)
result = self.execute_generic_command(cmd)
if result != None:
return result
else:
if len(self._group_name) > 0:
return self.execute_group_command(
self._gdict[self._group_name], cmd)
else:
return (
MoveGroupInfoLevel.INFO,
self.get_help() +
"\n\nNo groups initialized yet. You must call the 'use' or the 'load' command first.\n",
)
def execute_generic_command(self, cmd):
if os.path.isfile("cmd/" + cmd):
cmd = "load cmd/" + cmd
cmdlow = cmd.lower()
if cmdlow.startswith("use"):
if cmdlow == "use":
return (MoveGroupInfoLevel.INFO,
"\n".join(self.get_loaded_groups()))
clist = cmd.split()
clist[0] = clist[0].lower()
if len(clist) == 2:
if clist[0] == "use":
if clist[1] == "previous":
clist[1] = self._prev_group_name
if len(clist[1]) == 0:
return (MoveGroupInfoLevel.DEBUG, "OK")
if clist[1] in self._gdict:
self._prev_group_name = self._group_name
self._group_name = clist[1]
return (MoveGroupInfoLevel.DEBUG, "OK")
else:
try:
mg = MoveGroupCommander(clist[1])
self._gdict[clist[1]] = mg
self._group_name = clist[1]
return (MoveGroupInfoLevel.DEBUG, "OK")
except MoveItCommanderException as e:
return (
MoveGroupInfoLevel.FAIL,
"Initializing " + clist[1] + ": " + str(e),
)
except:
return (
MoveGroupInfoLevel.FAIL,
"Unable to initialize " + clist[1],
)
elif cmdlow.startswith("trace"):
if cmdlow == "trace":
return (
MoveGroupInfoLevel.INFO,
"trace is on" if self._trace else "trace is off",
)
clist = cmdlow.split()
if clist[0] == "trace" and clist[1] == "on":
self._trace = True
return (MoveGroupInfoLevel.DEBUG, "OK")
if clist[0] == "trace" and clist[1] == "off":
self._trace = False
return (MoveGroupInfoLevel.DEBUG, "OK")
elif cmdlow.startswith("load"):
filename = self.DEFAULT_FILENAME
clist = cmd.split()
if len(clist) > 2:
return (MoveGroupInfoLevel.WARN,
"Unable to parse load command")
if len(clist) == 2:
filename = clist[1]
if not os.path.isfile(filename):
filename = "cmd/" + filename
line_num = 0
line_content = ""
try:
f = open(filename, "r")
for line in f:
line_num += 1
line = line.rstrip()
line_content = line
if self._trace:
print("%s:%d: %s" %
(filename, line_num, line_content))
comment = line.find("#")
if comment != -1:
line = line[0:comment].rstrip()
if line != "":
self.execute(line.rstrip())
line_content = ""
return (MoveGroupInfoLevel.DEBUG, "OK")
except MoveItCommanderException as e:
if line_num > 0:
return (
MoveGroupInfoLevel.WARN,
"Error at %s:%d: %s\n%s" %
(filename, line_num, line_content, str(e)),
)
else:
return (
MoveGroupInfoLevel.WARN,
"Processing " + filename + ": " + str(e),
)
except:
if line_num > 0:
return (
MoveGroupInfoLevel.WARN,
"Error at %s:%d: %s" %
(filename, line_num, line_content),
)
else:
return (MoveGroupInfoLevel.WARN,
"Unable to load " + filename)
elif cmdlow.startswith("save"):
filename = self.DEFAULT_FILENAME
clist = cmd.split()
if len(clist) > 2:
return (MoveGroupInfoLevel.WARN,
"Unable to parse save command")
if len(clist) == 2:
filename = clist[1]
try:
f = open(filename, "w")
for gr in self._gdict.keys():
f.write("use " + gr + "\n")
known = self._gdict[gr].get_remembered_joint_values()
for v in known.keys():
f.write(v + " = [" +
" ".join([str(x) for x in known[v]]) + "]\n")
if self._db_host != None:
f.write("database " + self._db_host + " " +
str(self._db_port) + "\n")
return (MoveGroupInfoLevel.DEBUG, "OK")
except:
return (MoveGroupInfoLevel.WARN, "Unable to save " + filename)
else:
return None
def execute_group_command(self, g, cmdorig):
cmd = cmdorig.lower()
if cmd == "stop":
g.stop()
return (MoveGroupInfoLevel.DEBUG, "OK")
if cmd == "id":
return (MoveGroupInfoLevel.INFO, g.get_name())
if cmd == "help":
return (MoveGroupInfoLevel.INFO, self.get_help())
if cmd == "vars":
known = g.get_remembered_joint_values()
return (MoveGroupInfoLevel.INFO,
"[" + " ".join(known.keys()) + "]")
if cmd == "joints":
joints = g.get_joints()
return (
MoveGroupInfoLevel.INFO,
"\n" + "\n".join(
[str(i) + " = " + joints[i]
for i in range(len(joints))]) + "\n",
)
if cmd == "show":
return self.command_show(g)
if cmd == "current":
return self.command_current(g)
if cmd == "ground":
pose = PoseStamped()
pose.pose.position.x = 0
pose.pose.position.y = 0
# offset such that the box is 0.1 mm below ground (to prevent collision with the robot itself)
pose.pose.position.z = -0.0501
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
pose.header.stamp = rospy.get_rostime()
pose.header.frame_id = self._robot.get_root_link()
self._planning_scene_interface.attach_box(
self._robot.get_root_link(), "ground", pose, (3, 3, 0.1))
return (MoveGroupInfoLevel.INFO, "Added ground")
if cmd == "eef":
if len(g.get_end_effector_link()) > 0:
return (MoveGroupInfoLevel.INFO, g.get_end_effector_link())
else:
return (MoveGroupInfoLevel.INFO,
"There is no end effector defined")
if cmd == "database":
if self._db_host == None:
return (MoveGroupInfoLevel.INFO, "Not connected to a database")
else:
return (
MoveGroupInfoLevel.INFO,
"Connected to " + self._db_host + ":" + str(self._db_port),
)
if cmd == "plan":
if self._last_plan != None:
return (MoveGroupInfoLevel.INFO, str(self._last_plan))
else:
return (MoveGroupInfoLevel.INFO, "No previous plan")
if cmd == "constrain":
g.clear_path_constraints()
return (MoveGroupInfoLevel.SUCCESS, "Cleared path constraints")
if cmd == "tol" or cmd == "tolerance":
return (
MoveGroupInfoLevel.INFO,
"Joint = %0.6g, Position = %0.6g, Orientation = %0.6g" %
g.get_goal_tolerance(),
)
if cmd == "time":
return (MoveGroupInfoLevel.INFO, str(g.get_planning_time()))
if cmd == "execute":
if self._last_plan != None:
if g.execute(self._last_plan):
return (MoveGroupInfoLevel.SUCCESS,
"Plan submitted for execution")
else:
return (
MoveGroupInfoLevel.WARN,
"Failed to submit plan for execution",
)
else:
return (MoveGroupInfoLevel.WARN, "No motion plan computed")
# see if we have assignment between variables
assign_match = re.match(r"^(\w+)\s*=\s*(\w+)$", cmd)
if assign_match:
known = g.get_remembered_joint_values()
if assign_match.group(2) in known:
g.remember_joint_values(assign_match.group(1),
known[assign_match.group(2)])
return (
MoveGroupInfoLevel.SUCCESS,
assign_match.group(1) + " is now the same as " +
assign_match.group(2),
)
else:
return (MoveGroupInfoLevel.WARN,
"Unknown command: '" + cmd + "'")
# see if we have assignment of matlab-like vector syntax
set_match = re.match(r"^(\w+)\s*=\s*\[([\d\.e\-\+\s]+)\]$", cmd)
if set_match:
try:
g.remember_joint_values(
set_match.group(1),
[float(x) for x in set_match.group(2).split()])
return (
MoveGroupInfoLevel.SUCCESS,
"Remembered joint values [" + set_match.group(2) +
"] under the name " + set_match.group(1),
)
except:
return (
MoveGroupInfoLevel.WARN,
"Unable to parse joint value [" + set_match.group(2) + "]",
)
# see if we have assignment of matlab-like element update
component_match = re.match(
r"^(\w+)\s*\[\s*(\d+)\s*\]\s*=\s*([\d\.e\-\+]+)$", cmd)
if component_match:
known = g.get_remembered_joint_values()
if component_match.group(1) in known:
try:
val = known[component_match.group(1)]
val[int(component_match.group(2))] = float(
component_match.group(3))
g.remember_joint_values(component_match.group(1), val)
return (
MoveGroupInfoLevel.SUCCESS,
"Updated " + component_match.group(1) + "[" +
component_match.group(2) + "]",
)
except:
return (
MoveGroupInfoLevel.WARN,
"Unable to parse index or value in '" + cmd + "'",
)
else:
return (MoveGroupInfoLevel.WARN,
"Unknown command: '" + cmd + "'")
clist = cmdorig.split()
clist[0] = clist[0].lower()
# if this is an unknown one-word command, it is probably a variable
if len(clist) == 1:
known = g.get_remembered_joint_values()
if cmd in known:
return (
MoveGroupInfoLevel.INFO,
"[" + " ".join([str(x) for x in known[cmd]]) + "]",
)
else:
return (MoveGroupInfoLevel.WARN,
"Unknown command: '" + cmd + "'")
# command with one argument
if len(clist) == 2:
if clist[0] == "go":
self._last_plan = None
if clist[1] == "rand" or clist[1] == "random":
vals = g.get_random_joint_values()
g.set_joint_value_target(vals)
if g.go():
return (
MoveGroupInfoLevel.SUCCESS,
"Moved to random target [" +
" ".join([str(x) for x in vals]) + "]",
)
else:
return (
MoveGroupInfoLevel.FAIL,
"Failed while moving to random target [" +
" ".join([str(x) for x in vals]) + "]",
)
else:
try:
g.set_named_target(clist[1])
if g.go():
return (MoveGroupInfoLevel.SUCCESS,
"Moved to " + clist[1])
else:
return (
MoveGroupInfoLevel.FAIL,
"Failed while moving to " + clist[1],
)
except MoveItCommanderException as e:
return (
MoveGroupInfoLevel.WARN,
"Going to " + clist[1] + ": " + str(e),
)
except:
return (MoveGroupInfoLevel.WARN,
clist[1] + " is unknown")
if clist[0] == "plan":
self._last_plan = None
vals = None
if clist[1] == "rand" or clist[1] == "random":
vals = g.get_random_joint_values()
g.set_joint_value_target(vals)
self._last_plan = g.plan()[1] # The trajectory msg
else:
try:
g.set_named_target(clist[1])
self._last_plan = g.plan()[1] # The trajectory msg
except MoveItCommanderException as e:
return (
MoveGroupInfoLevel.WARN,
"Planning to " + clist[1] + ": " + str(e),
)
except:
return (MoveGroupInfoLevel.WARN,
clist[1] + " is unknown")
if self._last_plan != None:
if (len(self._last_plan.joint_trajectory.points) == 0
and len(self._last_plan.multi_dof_joint_trajectory.
points) == 0):
self._last_plan = None
dest_str = clist[1]
if vals != None:
dest_str = "[" + " ".join([str(x) for x in vals]) + "]"
if self._last_plan != None:
return (MoveGroupInfoLevel.SUCCESS,
"Planned to " + dest_str)
else:
return (
MoveGroupInfoLevel.FAIL,
"Failed while planning to " + dest_str,
)
elif clist[0] == "pick":
self._last_plan = None
if g.pick(clist[1]):
return (MoveGroupInfoLevel.SUCCESS,
"Picked object " + clist[1])
else:
return (
MoveGroupInfoLevel.FAIL,
"Failed while trying to pick object " + clist[1],
)
elif clist[0] == "place":
self._last_plan = None
if g.place(clist[1]):
return (MoveGroupInfoLevel.SUCCESS,
"Placed object " + clist[1])
else:
return (
MoveGroupInfoLevel.FAIL,
"Failed while trying to place object " + clist[1],
)
elif clist[0] == "planner":
g.set_planner_id(clist[1])
return (MoveGroupInfoLevel.SUCCESS,
"Planner is now " + clist[1])
elif clist[0] == "record" or clist[0] == "rec":
g.remember_joint_values(clist[1])
return (
MoveGroupInfoLevel.SUCCESS,
"Remembered current joint values under the name " +
clist[1],
)
elif clist[0] == "rand" or clist[0] == "random":
g.remember_joint_values(clist[1], g.get_random_joint_values())
return (
MoveGroupInfoLevel.SUCCESS,
"Remembered random joint values under the name " +
clist[1],
)
elif clist[0] == "del" or clist[0] == "delete":
g.forget_joint_values(clist[1])
return (
MoveGroupInfoLevel.SUCCESS,
"Forgot joint values under the name " + clist[1],
)
elif clist[0] == "show":
known = g.get_remembered_joint_values()
if clist[1] in known:
return (
MoveGroupInfoLevel.INFO,
"[" + " ".join([str(x)
for x in known[clist[1]]]) + "]",
)
else:
return (
MoveGroupInfoLevel.WARN,
"Joint values for " + clist[1] + " are not known",
)
elif clist[0] == "tol" or clist[0] == "tolerance":
try:
g.set_goal_tolerance(float(clist[1]))
return (MoveGroupInfoLevel.SUCCESS, "OK")
except:
return (
MoveGroupInfoLevel.WARN,
"Unable to parse tolerance value '" + clist[1] + "'",
)
elif clist[0] == "time":
try:
g.set_planning_time(float(clist[1]))
return (MoveGroupInfoLevel.SUCCESS, "OK")
except:
return (
MoveGroupInfoLevel.WARN,
"Unable to parse planning duration value '" +
clist[1] + "'",
)
elif clist[0] == "constrain":
try:
g.set_path_constraints(clist[1])
return (MoveGroupInfoLevel.SUCCESS, "OK")
except:
if self._db_host != None:
return (
MoveGroupInfoLevel.WARN,
"Constraint " + clist[1] + " is not known.",
)
else:
return (MoveGroupInfoLevel.WARN,
"Not connected to a database.")
elif clist[0] == "wait":
try:
time.sleep(float(clist[1]))
return (MoveGroupInfoLevel.SUCCESS,
clist[1] + " seconds passed")
except:
return (
MoveGroupInfoLevel.WARN,
"Unable to wait '" + clist[1] + "' seconds",
)
elif clist[0] == "database":
try:
g.set_constraints_database(clist[1], self._db_port)
self._db_host = clist[1]
return (
MoveGroupInfoLevel.SUCCESS,
"Connected to " + self._db_host + ":" +
str(self._db_port),
)
except:
return (
MoveGroupInfoLevel.WARN,
"Unable to connect to '" + clist[1] + ":" +
str(self._db_port) + "'",
)
else:
return (MoveGroupInfoLevel.WARN,
"Unknown command: '" + cmd + "'")
if len(clist) == 3:
if clist[0] == "go" and clist[1] in self.GO_DIRS:
self._last_plan = None
try:
offset = float(clist[2])
(axis, factor) = self.GO_DIRS[clist[1]]
return self.command_go_offset(g, offset, factor, axis,
clist[1])
except MoveItCommanderException as e:
return (
MoveGroupInfoLevel.WARN,
"Going " + clist[1] + ": " + str(e),
)
except:
return (
MoveGroupInfoLevel.WARN,
"Unable to parse distance '" + clist[2] + "'",
)
elif clist[0] == "allow" and (clist[1] == "look"
or clist[1] == "looking"):
if (clist[2] == "1" or clist[2] == "true" or clist[2] == "T"
or clist[2] == "True"):
g.allow_looking(True)
else:
g.allow_looking(False)
return (MoveGroupInfoLevel.DEBUG, "OK")
elif clist[0] == "allow" and (clist[1] == "replan"
or clist[1] == "replanning"):
if (clist[2] == "1" or clist[2] == "true" or clist[2] == "T"
or clist[2] == "True"):
g.allow_replanning(True)
else:
g.allow_replanning(False)
return (MoveGroupInfoLevel.DEBUG, "OK")
elif clist[0] == "database":
try:
g.set_constraints_database(clist[1], int(clist[2]))
self._db_host = clist[1]
self._db_port = int(clist[2])
return (
MoveGroupInfoLevel.SUCCESS,
"Connected to " + self._db_host + ":" +
str(self._db_port),
)
except:
self._db_host = None
return (
MoveGroupInfoLevel.WARN,
"Unable to connect to '" + clist[1] + ":" + clist[2] +
"'",
)
if len(clist) == 4:
if clist[0] == "rotate":
try:
g.set_rpy_target([float(x) for x in clist[1:]])
if g.go():
return (MoveGroupInfoLevel.SUCCESS,
"Rotation complete")
else:
return (
MoveGroupInfoLevel.FAIL,
"Failed while rotating to " + " ".join(clist[1:]),
)
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, str(e))
except:
return (
MoveGroupInfoLevel.WARN,
"Unable to parse X-Y-Z rotation values '" +
" ".join(clist[1:]) + "'",
)
if len(clist) >= 7:
if clist[0] == "go":
self._last_plan = None
approx = False
if len(clist) > 7:
if clist[7] == "approx" or clist[7] == "approximate":
approx = True
try:
p = Pose()
p.position.x = float(clist[1])
p.position.y = float(clist[2])
p.position.z = float(clist[3])
q = tf.transformations.quaternion_from_euler(
float(clist[4]), float(clist[5]), float(clist[6]))
p.orientation.x = q[0]
p.orientation.y = q[1]
p.orientation.z = q[2]
p.orientation.w = q[3]
if approx:
g.set_joint_value_target(p, True)
else:
g.set_pose_target(p)
if g.go():
return (
MoveGroupInfoLevel.SUCCESS,
"Moved to pose target\n%s\n" % (str(p)),
)
else:
return (
MoveGroupInfoLevel.FAIL,
"Failed while moving to pose \n%s\n" % (str(p)),
)
except MoveItCommanderException as e:
return (
MoveGroupInfoLevel.WARN,
"Going to pose target: %s" % (str(e)),
)
except:
return (
MoveGroupInfoLevel.WARN,
"Unable to parse pose '" + " ".join(clist[1:]) + "'",
)
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
def command_show(self, g):
known = g.get_remembered_joint_values()
res = []
for k in known.keys():
res.append(k + " = [" + " ".join([str(x) for x in known[k]]) + "]")
return (MoveGroupInfoLevel.INFO, "\n".join(res))
def command_current(self, g):
res = ("joints = [" +
", ".join([str(x) for x in g.get_current_joint_values()]) + "]")
if len(g.get_end_effector_link()) > 0:
res = (res + "\n\n" + g.get_end_effector_link() + " pose = [\n" +
str(g.get_current_pose()) + " ]")
res = (res + "\n" + g.get_end_effector_link() + " RPY = " +
str(g.get_current_rpy()))
return (MoveGroupInfoLevel.INFO, res)
def command_go_offset(self, g, offset, factor, dimension_index,
direction_name):
if g.has_end_effector_link():
try:
g.shift_pose_target(dimension_index, factor * offset)
if g.go():
return (
MoveGroupInfoLevel.SUCCESS,
"Moved " + direction_name + " by " + str(offset) +
" m",
)
else:
return (
MoveGroupInfoLevel.FAIL,
"Failed while moving " + direction_name,
)
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, str(e))
except:
return (MoveGroupInfoLevel.WARN,
"Unable to process pose update")
else:
return (
MoveGroupInfoLevel.WARN,
"No known end effector. Cannot move " + direction_name,
)
def resolve_command_alias(self, cmdorig):
cmd = cmdorig.lower()
if cmd == "which":
return "id"
if cmd == "groups":
return "use"
return cmdorig
def get_help(self):
res = []
res.append("Known commands:")
res.append(" help show this screen")
res.append(
" allow looking <true|false> enable/disable looking around")
res.append(
" allow replanning <true|false> enable/disable replanning")
res.append(" constrain clear path constraints")
res.append(
" constrain <name> use the constraint <name> as a path constraint"
)
res.append(
" current show the current state of the active group")
res.append(
" database display the current database connection (if any)"
)
res.append(
" delete <name> forget the joint values under the name <name>"
)
res.append(
" eef print the name of the end effector attached to the current group"
)
res.append(
" execute execute a previously computed motion plan")
res.append(
" go <name> plan and execute a motion to the state <name>"
)
res.append(
" go rand plan and execute a motion to a random state"
)
res.append(
" go <dir> <dx>| plan and execute a motion in direction up|down|left|right|forward|backward for distance <dx>"
)
res.append(
" ground add a ground plane to the planning scene")
res.append(
" id|which display the name of the group that is operated on"
)
res.append(
" joints display names of the joints in the active group"
)
res.append(
" load [<file>] load a set of interpreted commands from a file"
)
res.append(" pick <name> pick up object <name>")
res.append(" place <name> place object <name>")
res.append(" plan <name> plan a motion to the state <name>")
res.append(" plan rand plan a motion to a random state")
res.append(
" planner <name> use planner <name> to plan next motion")
res.append(
" record <name> record the current joint values under the name <name>"
)
res.append(
" rotate <x> <y> <z> plan and execute a motion to a specified orientation (about the X,Y,Z axes)"
)
res.append(
" save [<file>] save the currently known variables as a set of commands"
)
res.append(
" show display the names and values of the known states"
)
res.append(" show <name> display the value of a state")
res.append(" stop stop the active group")
res.append(
" time show the configured allowed planning time")
res.append(" time <val> set the allowed planning time")
res.append(
" tolerance show the tolerance for reaching the goal region"
)
res.append(
" tolerance <val> set the tolerance for reaching the goal region"
)
res.append(
" trace <on|off> enable/disable replanning or looking around"
)
res.append(
" use <name> switch to using the group named <name> (and load it if necessary)"
)
res.append(
" use|groups show the group names that are already loaded"
)
res.append(
" vars display the names of the known states")
res.append(" wait <dt> sleep for <dt> seconds")
res.append(" x = y assign the value of y to x")
res.append(" x = [v1 v2...] assign a vector of values to x")
res.append(
" x[idx] = val assign a value to dimension idx of x")
return "\n".join(res)
def get_keywords(self):
known_vars = []
known_constr = []
if self.get_active_group() != None:
known_vars = self.get_active_group().get_remembered_joint_values(
).keys()
known_constr = self.get_active_group().get_known_constraints()
groups = self._robot.get_group_names()
return {
"go":
["up", "down", "left", "right", "backward", "forward", "random"] +
list(known_vars),
"help": [],
"record":
known_vars,
"show":
known_vars,
"wait": [],
"delete":
known_vars,
"database": [],
"current": [],
"use":
groups,
"load": [],
"save": [],
"pick": [],
"place": [],
"plan":
known_vars,
"allow": ["replanning", "looking"],
"constrain":
known_constr,
"vars": [],
"joints": [],
"tolerance": [],
"time": [],
"eef": [],
"execute": [],
"ground": [],
"id": [],
}
class BaxterMoveit(BaxterRobot):
LEFT = -1
RIGHT = 1
def __init__(self, node, limb=+1):
super(BaxterMoveit, self).__init__(node, limb=+1)
self.group = MoveGroupCommander(self.lns + "_arm")
self.robot = RobotCommander()
self.scene = PlanningSceneInterface()
self._info()
def _info(self):
'''Getting Basic Information'''
# name of the reference frame for this robot:
print("@@@@@@@@@@@@ Reference frame: %s" %
self.group.get_planning_frame())
# We can also print the name of the end-effector link for this group:
print("@@@@@@@@@@@@ End effector: %s" %
self.group.get_end_effector_link())
# We can get a list of all the groups in the robot:
print("@@@@@@@@@@@@ Robot Groups: @@@@@@@@@@@@@",
self.robot.get_group_names())
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print("@@@@@@@@@@@@ Printing robot state @@@@@@@@@@@@@")
print(self.robot.get_current_state())
def obstacle(self, name, position, size):
planning_frame = self.robot.get_planning_frame()
pose = PoseStamped()
pose.header.frame_id = planning_frame
pose.pose.position.x = position[0]
pose.pose.position.y = position[1]
pose.pose.position.z = position[2]
self.scene.add_box(name, pose, tuple(size))
def movej(self, q, raw=False):
''' move in joint space by giving a joint configuration as dictionary'''
if raw:
print("in moveit 'raw' motion is not avaiable")
# succ = False
# while succ is False:
succ = self.group.go(q, wait=True)
# self.group.stop() # ensures that there is no residual movement
def showplan(self, target):
if type(target) is PyKDL.Frame or type(target) is Pose:
q = self.ik(target)
elif type(target) is dict:
q = target
else:
print("Target format error")
return
self.group.set_joint_value_target(q)
self.group.plan()
def movep(self, pose, raw=False):
''' move the eef in Cartesian space by giving a geometry_msgs.Pose or a PyKDL.Frame'''
if type(pose) is PyKDL.Frame:
pose = transformations.KDLToPose(pose)
q = self.ik(pose)
if q is not None:
self.movej(q, raw=raw)
else:
print("\nNO SOLUTION TO IK\n" * 20)
class TestDualarmMoveit(unittest.TestCase):
_KINEMATICSOLVER_SAFE = 'RRTConnectkConfigDefault'
_MOVEGROUP_NAME_TORSO = 'torso'
_JOINTNAMES_TORSO = ['CHEST_JOINT0']
# Set of MoveGroup name and the list of joints
_MOVEGROUP_ATTR_TORSO = [_MOVEGROUP_NAME_TORSO, _JOINTNAMES_TORSO]
_MOVEGROUP_NAME_LEFTARM = 'left_arm'
_JOINTNAMES_LEFTARM = [
'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3',
'LARM_JOINT4', 'LARM_JOINT5'
]
_MOVEGROUP_ATTR_LEFTARM = [_MOVEGROUP_NAME_LEFTARM, _JOINTNAMES_LEFTARM]
_MOVEGROUP_NAME_RIGHTARM = 'right_arm'
_JOINTNAMES_RIGHTARM = [
'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3',
'RARM_JOINT4', 'RARM_JOINT5'
]
_MOVEGROUP_ATTR_RIGHTARM = [_MOVEGROUP_NAME_RIGHTARM, _JOINTNAMES_RIGHTARM]
_MOVEGROUP_NAME_LEFTARM_TORSO = 'left_arm_torso'
_JOINTNAMES_LEFTARM_TORSO = _JOINTNAMES_TORSO + _JOINTNAMES_LEFTARM
_MOVEGROUP_ATTR_LEFTARM_TORSO = [
_MOVEGROUP_NAME_LEFTARM_TORSO, _JOINTNAMES_LEFTARM_TORSO
]
_MOVEGROUP_NAME_RIGHTARM_TORSO = 'right_arm_torso'
_JOINTNAMES_RIGHTARM_TORSO = _JOINTNAMES_TORSO + _JOINTNAMES_RIGHTARM
_MOVEGROUP_ATTR_RIGHTARM_TORSO = [
_MOVEGROUP_NAME_RIGHTARM_TORSO, _JOINTNAMES_RIGHTARM_TORSO
]
_MOVEGROUP_NAME_LEFTHAND = 'left_hand'
_JOINTNAMES_LEFTHAND = ['LARM_JOINT5']
_MOVEGROUP_ATTR_LEFTHAND = [_MOVEGROUP_NAME_LEFTHAND, _JOINTNAMES_LEFTHAND]
_MOVEGROUP_NAME_RIGHTHAND = 'right_hand'
_JOINTNAMES_RIGHTHAND = ['RARM_JOINT5']
_MOVEGROUP_ATTR_RIGHTHAND = [
_MOVEGROUP_NAME_RIGHTHAND, _JOINTNAMES_RIGHTHAND
]
_MOVEGROUP_NAME_HEAD = 'head'
_JOINTNAMES_HEAD = ['HEAD_JOINT0', 'HEAD_JOINT1']
_MOVEGROUP_ATTR_HEAD = [_MOVEGROUP_NAME_HEAD, _JOINTNAMES_HEAD]
_MOVEGROUP_NAME_BOTHARMS = 'botharms'
## _JOINTNAMES_BOTHARMS = _JOINTNAMES_TORSO +_JOINTNAMES_LEFTARM + _JOINTNAMES_RIGHTARM
_JOINTNAMES_BOTHARMS = _JOINTNAMES_LEFTARM + _JOINTNAMES_RIGHTARM
_MOVEGROUP_ATTR_BOTHARMS = [_MOVEGROUP_NAME_BOTHARMS, _JOINTNAMES_BOTHARMS]
_MOVEGROUP_NAME_UPPERBODY = 'upperbody'
_JOINTNAMES_UPPERBODY = _JOINTNAMES_TORSO + _JOINTNAMES_HEAD + _JOINTNAMES_LEFTARM + _JOINTNAMES_RIGHTARM
_MOVEGROUP_ATTR_UPPERBODY = [
_MOVEGROUP_NAME_UPPERBODY, _JOINTNAMES_UPPERBODY
]
# List of all MoveGroup set
_MOVEGROUP_NAMES = sorted([
_MOVEGROUP_NAME_TORSO, _MOVEGROUP_NAME_HEAD, _MOVEGROUP_NAME_LEFTARM,
_MOVEGROUP_NAME_RIGHTARM, _MOVEGROUP_NAME_LEFTARM_TORSO,
_MOVEGROUP_NAME_RIGHTARM_TORSO, _MOVEGROUP_NAME_LEFTHAND,
_MOVEGROUP_NAME_RIGHTHAND, _MOVEGROUP_NAME_BOTHARMS,
_MOVEGROUP_NAME_UPPERBODY
])
_MOVEGROUP_ATTRS = [
_MOVEGROUP_ATTR_TORSO, _MOVEGROUP_ATTR_HEAD, _MOVEGROUP_ATTR_LEFTARM,
_MOVEGROUP_ATTR_RIGHTARM, _MOVEGROUP_ATTR_LEFTARM_TORSO,
_MOVEGROUP_ATTR_RIGHTARM_TORSO, _MOVEGROUP_ATTR_LEFTHAND,
_MOVEGROUP_ATTR_RIGHTHAND, _MOVEGROUP_ATTR_BOTHARMS,
_MOVEGROUP_ATTR_UPPERBODY
]
_TARGETPOSE_LEFT = [] # TODO fill this
# def __init__(self, mg_attrs_larm, mg_attrs_rarm):
# '''
# @param mg_attrs_larm: List of MoveGroup attributes.
# See the member variable for the semantics of the data type.
# @type mg_attrs_larm: [str, [str]]
# '''
@classmethod
def setUpClass(self):
rospy.init_node('test_moveit')
rospy.sleep(5) # intentinally wait for starting up hrpsys
self.robot = RobotCommander()
# TODO: Read groups from SRDF file ideally.
for mg_attr in self._MOVEGROUP_ATTRS:
mg = MoveGroupCommander(mg_attr[0])
# Temporary workaround of planner's issue similar to https://github.com/tork-a/rtmros_nextage/issues/170
mg.set_planner_id(self._KINEMATICSOLVER_SAFE)
# Allow replanning to increase the odds of a solution
mg.allow_replanning(True)
# increase planning time
mg.set_planning_time(30.0)
# Append MoveGroup instance to the MoveGroup attribute list.
mg_attr.append(mg)
@classmethod
def tearDownClass(self):
True # TODO impl something meaningful
def _set_sample_pose(self):
'''
@return: Pose() with some values populated in.
'''
pose_target = Pose()
pose_target.orientation.x = 0.00
pose_target.orientation.y = 0.00
pose_target.orientation.z = -0.20
pose_target.orientation.w = 0.98
pose_target.position.x = 0.18
pose_target.position.y = -0.00
pose_target.position.z = 0.94
return pose_target
def _plan(self):
'''
Run `clear_pose_targets` at the beginning.
@rtype: RobotTrajectory http://docs.ros.org/api/moveit_msgs/html/msg/RobotTrajectory.html
'''
self._mvgroup.clear_pose_targets()
pose_target = self._set_sample_pose()
self._mvgroup.set_pose_target(pose_target)
plan = self._mvgroup.plan()
rospy.loginfo(' plan: {}'.format(plan))
return plan
def test_list_movegroups(self):
'''Check if the defined move groups are loaded.'''
self.assertEqual(self._MOVEGROUP_NAMES,
sorted(self.robot.get_group_names()))
def test_list_activejoints(self):
'''Check if the defined joints in a move group are loaded.'''
for mg_attr in self._MOVEGROUP_ATTRS:
self.assertEqual(
mg_attr[1], # joint groups for a Move Group.
sorted(mg_attr[2].get_active_joints()))
def _plan_leftarm(self, movegroup):
'''
@type movegroup: MoveGroupCommander
@rtype: RobotTrajectory http://docs.ros.org/api/moveit_msgs/html/msg/RobotTrajectory.html
'''
movegroup.clear_pose_targets()
pose_target = Pose()
pose_target.orientation.x = -0.32136357
pose_target.orientation.y = -0.63049522
pose_target.orientation.z = 0.3206799
pose_target.orientation.w = 0.62957575
pose_target.position.x = 0.32529
pose_target.position.y = 0.29919
pose_target.position.z = 0.24389
movegroup.set_pose_target(pose_target)
plan = movegroup.plan() # TODO catch exception
rospy.loginfo('Plan: {}'.format(plan))
return plan
def test_plan_success(self):
'''Evaluate plan (RobotTrajectory)'''
plan = self._plan_leftarm(self._MOVEGROUP_ATTR_LEFTARM[2])
# TODO Better way to check the plan is valid.
# Currently the following checks if the number of waypoints is not zero,
# which (hopefully) indicates that a path is computed.
self.assertNotEqual(0, plan.joint_trajectory.points)
def test_planandexecute(self):
'''
Evaluate Plan and Execute works.
Currently no value checking is done (, which is better to be implemented)
'''
mvgroup = self._MOVEGROUP_ATTR_LEFTARM[2]
self._plan_leftarm(mvgroup)
# TODO Better way to check the plan is valid.
# The following checks if plan execution was successful or not.
self.assertTrue(mvgroup.go() or mvgroup.go() or mvgroup.go())
def test_left_and_right_plan(self):
'''
Check if http://wiki.ros.org/rtmros_nextage/Tutorials/Programming_Hiro_NEXTAGE_OPEN_MOVEIT works
'''
larm = self._MOVEGROUP_ATTR_LEFTARM[2]
rarm = self._MOVEGROUP_ATTR_RIGHTARM[2]
#Right Arm Initial Pose
rarm_initial_pose = rarm.get_current_pose().pose
print "=" * 10, " Printing Right Hand initial pose: "
print rarm_initial_pose
#Light Arm Initial Pose
larm_initial_pose = larm.get_current_pose().pose
print "=" * 10, " Printing Left Hand initial pose: "
print larm_initial_pose
target_pose_r = Pose()
target_pose_r.position.x = 0.325471850974 - 0.01
target_pose_r.position.y = -0.182271241593 - 0.3
target_pose_r.position.z = 0.0676272396419 + 0.3
target_pose_r.orientation.x = -0.000556712307053
target_pose_r.orientation.y = -0.706576742941
target_pose_r.orientation.z = -0.00102461782513
target_pose_r.orientation.w = 0.707635461636
rarm.set_pose_target(target_pose_r)
print "=" * 10, " plan1 ..."
self.assertTrue(rarm.go() or rarm.go() or rarm.go())
rospy.sleep(1)
target_pose_l = [
target_pose_r.position.x, -target_pose_r.position.y,
target_pose_r.position.z, target_pose_r.orientation.x,
target_pose_r.orientation.y, target_pose_r.orientation.z,
target_pose_r.orientation.w
]
larm.set_pose_target(target_pose_l)
print "=" * 10, " plan2 ..."
self.assertTrue(larm.go() or larm.go() or larm.go())
rospy.sleep(1)
#Clear pose
rarm.clear_pose_targets()
#Right Hand
target_pose_r.position.x = 0.221486843301
target_pose_r.position.y = -0.0746407547512
target_pose_r.position.z = 0.642545484602
target_pose_r.orientation.x = 0.0669013615474
target_pose_r.orientation.y = -0.993519060661
target_pose_r.orientation.z = 0.00834224628291
target_pose_r.orientation.w = 0.0915122442864
rarm.set_pose_target(target_pose_r)
print "=" * 10, " plan3..."
self.assertTrue(rarm.go() or rarm.go() or rarm.go())
rospy.sleep(1)
print "=" * 10, "Initial pose ..."
rarm.set_pose_target(rarm_initial_pose)
larm.set_pose_target(larm_initial_pose)
self.assertTrue(rarm.go() or rarm.go() or rarm.go())
self.assertTrue(larm.go() or larm.go() or larm.go())
def test_botharms_plan(self):
botharms = self._MOVEGROUP_ATTR_BOTHARMS[2]
target_pose_r = Pose()
target_pose_l = Pose()
q = quaternion_from_euler(0, -math.pi / 2, 0)
target_pose_r.position.x = 0.3
target_pose_r.position.y = 0.1
target_pose_r.position.z = 0.0
target_pose_r.orientation.x = q[0]
target_pose_r.orientation.y = q[1]
target_pose_r.orientation.z = q[2]
target_pose_r.orientation.w = q[3]
target_pose_l.position.x = 0.3
target_pose_l.position.y = -0.1
target_pose_l.position.z = 0.3
target_pose_l.orientation.x = q[0]
target_pose_l.orientation.y = q[1]
target_pose_l.orientation.z = q[2]
target_pose_l.orientation.w = q[3]
botharms.set_pose_target(target_pose_r, 'RARM_JOINT5_Link')
botharms.set_pose_target(target_pose_l, 'LARM_JOINT5_Link')
self.assertTrue(botharms.go() or botharms.go() or botharms.go())
rospy.sleep(1)
target_pose_r.position.x = 0.3
target_pose_r.position.y = -0.2
target_pose_r.position.z = 0.0
target_pose_l.position.x = 0.3
target_pose_l.position.y = 0.2
target_pose_l.position.z = 0.0
botharms.set_pose_target(target_pose_r, 'RARM_JOINT5_Link')
botharms.set_pose_target(target_pose_l, 'LARM_JOINT5_Link')
self.assertTrue(botharms.go() or botharms.go() or botharms.go())
class pick_place():
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.ur5 = MoveGroupCommander("manipulator")
self.gripper = MoveGroupCommander("end_effector")
print("======================================================")
print(self.robot.get_group_names())
print(self.robot.get_link_names())
print(self.robot.get_joint_names())
print(self.robot.get_planning_frame())
print(self.ur5.get_end_effector_link())
print("======================================================")
self.ur5.set_max_velocity_scaling_factor(0.2)
self.ur5.set_max_acceleration_scaling_factor(0.2)
self.ur5.set_end_effector_link("fts_toolside")
self.ur5.set_planning_time(10.0)
#self.ur5.set_planner_id("RRTkConfigDefault")
self.gripper_ac = RobotiqActionClient('icl_phri_gripper/gripper_controller')
self.gripper_ac.wait_for_server()
self.gripper_ac.initiate()
self.gripper_ac.send_goal(0.08)
self.gripper_ac.wait_for_result()
self.sub = rospy.Subscriber('/target_position', Int32MultiArray, self.pickplace_cb)
def single_exuete(self, position, mode):
offset = 0.01
rospy.loginfo("let do a single exuete")
rospy.sleep(1)
position_copy = deepcopy(position)
position_copy += [0.14]
position_copy[1] = position_copy[1] + offset
pre_position = define_grasp([position_copy[0], position_copy[1], position_copy[2] + 0.2])
post_position = define_grasp([position_copy[0], position_copy[1], position_copy[2] + 0.2])
grasp_position = define_grasp(position_copy)
self.ur5.set_pose_target(pre_position)
rospy.loginfo("let's go to pre position")
self.ur5.go()
self.ur5.stop()
self.ur5.clear_pose_targets()
rospy.sleep(1)
self.ur5.set_pose_target(grasp_position)
rospy.loginfo("let's do this")
self.ur5.go()
self.ur5.stop()
self.ur5.clear_pose_targets()
if mode == "pick":
self.gripper_ac.send_goal(0)
if mode == "place":
self.gripper_ac.send_goal(0.08)
self.gripper_ac.wait_for_result()
rospy.loginfo("I got this")
rospy.sleep(1)
self.ur5.set_pose_target(post_position)
rospy.loginfo("move out")
self.ur5.go()
self.ur5.stop()
self.ur5.clear_pose_targets()
rospy.sleep(1)
def pair_exuete(self, pick_position, place_position):
rospy.loginfo("here we go pair")
if pick_position and place_position:
self.single_exuete(pick_position, "pick")
self.single_exuete(place_position, "place")
#rospy.sleep(1)
rospy.loginfo("let's go and get some rest")
rest_position = define_grasp([0.486, -0.152, 0.342])
self.ur5.set_pose_target(rest_position)
self.ur5.go()
self.ur5.stop()
self.ur5.clear_pose_targets()
rospy.sleep(1)
def pickplace_cb(self, msg):
#print(msg)
print(msg.data)
a = list(msg.data)
mean_x = np.mean([a[i] for i in range(0, len(a)-2, 2)])
mean_y = np.mean([a[i] for i in range(1, len(a)-2, 2)])
num_goals = (len(msg.data) -2)/2
rospy.loginfo("there is {} goals".format(num_goals))
for i in range(0, len(a)-2, 2):
pick_x, pick_y = coord_converter(msg.data[i], msg.data[i+1])
leeway_x = int(msg.data[i] - mean_x)
leeway_y = int(msg.data[i+1] - mean_y)
place_x, place_y = coord_converter(msg.data[-2] + leeway_x, msg.data[-1] + leeway_y)
print(pick_x, pick_y)
print(place_x, place_y)
self.pair_exuete([pick_x, pick_y], [place_x, place_y])
from moveit_commander import RobotCommander, MoveGroupCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint
import tf
if __name__=='__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("arm")
print "============ Robot Groups:"
print robot.get_group_names()
print "============ Robot Links for arm:"
print robot.get_link_names("arm")
print "============ Robot Links for gripper:"
print robot.get_link_names("gripper")
print right_arm.get_end_effector_link()
print "============ Printing robot state"
#print robot.get_current_state()
print "============"
rospy.sleep(1)
# clean the scene
scene.remove_world_object("pole")
scene.remove_world_object("table")
class TestMoveit(unittest.TestCase):
_MOVEGROUP_MAIN = 'manipulator'
_KINEMATICSOLVER_SAFE = 'RRTConnectkConfigDefault'
@classmethod
def setUpClass(self):
rospy.init_node('test_moveit_vs060')
self.robot = RobotCommander()
self._mvgroup = MoveGroupCommander(self._MOVEGROUP_MAIN)
# Temporary workaround of planner's issue similar to https://github.com/tork-a/rtmros_nextage/issues/170
self._mvgroup.set_planner_id(self._KINEMATICSOLVER_SAFE)
self._movegroups = sorted(['manipulator', 'manipulator_flange'])
self._joints_movegroup_main = sorted(
['j1', 'j2', 'j3', 'j4', 'j5', 'flange'])
@classmethod
def tearDownClass(self):
True # TODO impl something meaningful
def _set_sample_pose(self):
'''
@return: Pose() with some values populated in.
'''
pose_target = Pose()
pose_target.orientation.x = 0.00
pose_target.orientation.y = 0.00
pose_target.orientation.z = -0.20
pose_target.orientation.w = 0.98
pose_target.position.x = 0.18
pose_target.position.y = 0.18
pose_target.position.z = 0.94
return pose_target
def _plan(self):
'''
Run `clear_pose_targets` at the beginning.
@rtype: RobotTrajectory http://docs.ros.org/api/moveit_msgs/html/msg/RobotTrajectory.html
'''
self._mvgroup.clear_pose_targets()
pose_target = self._set_sample_pose()
self._mvgroup.set_pose_target(pose_target)
plan = self._mvgroup.plan()
rospy.loginfo(' plan: '.format(plan))
return plan
def test_list_movegroups(self):
'''Check if the defined move groups are loaded.'''
self.assertEqual(self._movegroups,
sorted(self.robot.get_group_names()))
def test_list_activejoints(self):
'''Check if the defined joints in a move group are loaded.'''
self.assertEqual(self._joints_movegroup_main,
sorted(self._mvgroup.get_active_joints()))
def test_plan(self):
'''Evaluate plan (RobotTrajectory)'''
plan = self._plan()
# TODO Better way to check the plan is valid.
# Currently the following checks if the number of waypoints is not zero,
# which (hopefully) indicates that a path is computed.
self.assertNotEqual(0, plan.joint_trajectory.points)
def test_planandexecute(self):
'''
Evaluate Plan and Execute works.
Currently no value checking is done (, which is better to be implemented)
'''
self._plan()
# TODO Better way to check the plan is valid.
# The following checks if plan execution was successful or not.
self.assertTrue(self._mvgroup.go())
def test_set_pose_target(self):
'''
Check for simple planning, originally testd in moved from denso_vs060_moveit_demo_test.py
'''
self._plan()
p = [0.1, -0.35, 0.4]
pose = PoseStamped(
header=rospy.Header(stamp=rospy.Time.now(), frame_id='/BASE'),
pose=Pose(position=Point(*p),
orientation=Quaternion(
*quaternion_from_euler(1.57, 0, 1.57, 'sxyz'))))
self._mvgroup.set_pose_target(pose)
self.assertTrue(self._mvgroup.go() or self._mvgroup.go())
def test_planning_scene(self):
'''
Check if publish_simple_scene.py is working
'''
rospy.wait_for_service('/get_planning_scene', timeout=20)
get_planning_scene = rospy.ServiceProxy("/get_planning_scene",
GetPlanningScene)
collision_objects = []
# wait for 10 sec
time_start = rospy.Time.now()
while not collision_objects and (rospy.Time.now() -
time_start).to_sec() < 10.0:
world = get_planning_scene(
PlanningSceneComponents(components=PlanningSceneComponents.
WORLD_OBJECT_NAMES)).scene.world
collision_objects = world.collision_objects
rospy.loginfo("collision_objects = " +
str(world.collision_objects))
rospy.sleep(1)
self.assertTrue(world.collision_objects != [], world)
class MoveGroupCommandInterpreter(object):
"""
Interpreter for simple commands
"""
DEFAULT_FILENAME = "move_group.cfg"
GO_DIRS = {"up" : (2,1), "down" : (2, -1), "z" : (2, 1),
"left" : (1, 1), "right" : (1, -1), "y" : (1, 1),
"forward" : (0, 1), "backward" : (0, -1), "back" : (0, -1), "x" : (0, 1) }
def __init__(self):
self._gdict = {}
self._group_name = ""
self._prev_group_name = ""
self._planning_scene_interface = PlanningSceneInterface()
self._robot = RobotCommander()
self._last_plan = None
self._db_host = None
self._db_port = 33829
self._trace = False
def get_active_group(self):
if len(self._group_name) > 0:
return self._gdict[self._group_name]
else:
return None
def get_loaded_groups(self):
return self._gdict.keys()
def execute(self, cmd):
cmd = self.resolve_command_alias(cmd)
result = self.execute_generic_command(cmd)
if result != None:
return result
else:
if len(self._group_name) > 0:
return self.execute_group_command(self._gdict[self._group_name], cmd)
else:
return (MoveGroupInfoLevel.INFO, self.get_help() + "\n\nNo groups initialized yet. You must call the 'use' or the 'load' command first.\n")
def execute_generic_command(self, cmd):
if os.path.isfile("cmd/" + cmd):
cmd = "load cmd/" + cmd
cmdlow = cmd.lower()
if cmdlow.startswith("use"):
if cmdlow == "use":
return (MoveGroupInfoLevel.INFO, "\n".join(self.get_loaded_groups()))
clist = cmd.split()
clist[0] = clist[0].lower()
if len(clist) == 2:
if clist[0] == "use":
if clist[1] == "previous":
clist[1] = self._prev_group_name
if len(clist[1]) == 0:
return (MoveGroupInfoLevel.DEBUG, "OK")
if self._gdict.has_key(clist[1]):
self._prev_group_name = self._group_name
self._group_name = clist[1]
return (MoveGroupInfoLevel.DEBUG, "OK")
else:
try:
mg = MoveGroupCommander(clist[1])
self._gdict[clist[1]] = mg
self._group_name = clist[1]
return (MoveGroupInfoLevel.DEBUG, "OK")
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.FAIL, "Initializing " + clist[1] + ": " + str(e))
except:
return (MoveGroupInfoLevel.FAIL, "Unable to initialize " + clist[1])
elif cmdlow.startswith("trace"):
if cmdlow == "trace":
return (MoveGroupInfoLevel.INFO, "trace is on" if self._trace else "trace is off")
clist = cmdlow.split()
if clist[0] == "trace" and clist[1] == "on":
self._trace = True
return (MoveGroupInfoLevel.DEBUG, "OK")
if clist[0] == "trace" and clist[1] == "off":
self._trace = False
return (MoveGroupInfoLevel.DEBUG, "OK")
elif cmdlow.startswith("load"):
filename = self.DEFAULT_FILENAME
clist = cmd.split()
if len(clist) > 2:
return (MoveGroupInfoLevel.WARN, "Unable to parse load command")
if len(clist) == 2:
filename = clist[1]
if not os.path.isfile(filename):
filename = "cmd/" + filename
line_num = 0
line_content = ""
try:
f = open(filename, 'r')
for line in f:
line_num += 1
line = line.rstrip()
line_content = line
if self._trace:
print ("%s:%d: %s" % (filename, line_num, line_content))
comment = line.find("#")
if comment != -1:
line = line[0:comment].rstrip()
if line != "":
self.execute(line.rstrip())
line_content = ""
return (MoveGroupInfoLevel.DEBUG, "OK")
except MoveItCommanderException as e:
if line_num > 0:
return (MoveGroupInfoLevel.WARN, "Error at %s:%d: %s\n%s" % (filename, line_num, line_content, str(e)))
else:
return (MoveGroupInfoLevel.WARN, "Processing " + filename + ": " + str(e))
except:
if line_num > 0:
return (MoveGroupInfoLevel.WARN, "Error at %s:%d: %s" % (filename, line_num, line_content))
else:
return (MoveGroupInfoLevel.WARN, "Unable to load " + filename)
elif cmdlow.startswith("save"):
filename = self.DEFAULT_FILENAME
clist = cmd.split()
if len(clist) > 2:
return (MoveGroupInfoLevel.WARN, "Unable to parse save command")
if len(clist) == 2:
filename = clist[1]
try:
f = open(filename, 'w')
for gr in self._gdict.keys():
f.write("use " + gr + "\n")
known = self._gdict[gr].get_remembered_joint_values()
for v in known.keys():
f.write(v + " = [" + " ".join([str(x) for x in known[v]]) + "]\n")
if self._db_host != None:
f.write("database " + self._db_host + " " + str(self._db_port) + "\n")
return (MoveGroupInfoLevel.DEBUG, "OK")
except:
return (MoveGroupInfoLevel.WARN, "Unable to save " + filename)
else:
return None
def execute_group_command(self, g, cmdorig):
cmd = cmdorig.lower()
if cmd == "stop":
g.stop()
return (MoveGroupInfoLevel.DEBUG, "OK")
if cmd == "id":
return (MoveGroupInfoLevel.INFO, g.get_name())
if cmd == "help":
return (MoveGroupInfoLevel.INFO, self.get_help())
if cmd == "vars":
known = g.get_remembered_joint_values()
return (MoveGroupInfoLevel.INFO, "[" + " ".join(known.keys()) + "]")
if cmd == "joints":
joints = g.get_joints()
return (MoveGroupInfoLevel.INFO, "\n" + "\n".join([str(i) + " = " + joints[i] for i in range(len(joints))]) + "\n")
if cmd == "show":
return self.command_show(g)
if cmd == "current":
return self.command_current(g)
if cmd == "ground":
pose = PoseStamped()
pose.pose.position.x = 0
pose.pose.position.y = 0
# offset such that the box is 0.1 mm below ground (to prevent collision with the robot itself)
pose.pose.position.z = -0.0501
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
pose.header.stamp = rospy.get_rostime()
pose.header.frame_id = self._robot.get_root_link()
self._planning_scene_interface.attach_box(self._robot.get_root_link(), "ground", pose, (3, 3, 0.1))
return (MoveGroupInfoLevel.INFO, "Added ground")
if cmd == "eef":
if len(g.get_end_effector_link()) > 0:
return (MoveGroupInfoLevel.INFO, g.get_end_effector_link())
else:
return (MoveGroupInfoLevel.INFO, "There is no end effector defined")
if cmd == "database":
if self._db_host == None:
return (MoveGroupInfoLevel.INFO, "Not connected to a database")
else:
return (MoveGroupInfoLevel.INFO, "Connected to " + self._db_host + ":" + str(self._db_port))
if cmd == "plan":
if self._last_plan != None:
return (MoveGroupInfoLevel.INFO, str(self._last_plan))
else:
return (MoveGroupInfoLevel.INFO, "No previous plan")
if cmd == "constrain":
g.clear_path_constraints()
return (MoveGroupInfoLevel.SUCCESS, "Cleared path constraints")
if cmd == "tol" or cmd == "tolerance":
return (MoveGroupInfoLevel.INFO, "Joint = %0.6g, Position = %0.6g, Orientation = %0.6g" % g.get_goal_tolerance())
if cmd == "time":
return (MoveGroupInfoLevel.INFO, str(g.get_planning_time()))
if cmd == "execute":
if self._last_plan != None:
if g.execute(self._last_plan):
return (MoveGroupInfoLevel.SUCCESS, "Plan submitted for execution")
else:
return (MoveGroupInfoLevel.WARN, "Failed to submit plan for execution")
else:
return (MoveGroupInfoLevel.WARN, "No motion plan computed")
# see if we have assignment between variables
assign_match = re.match(r"^(\w+)\s*=\s*(\w+)$", cmd)
if assign_match:
known = g.get_remembered_joint_values()
if known.has_key(assign_match.group(2)):
g.remember_joint_values(assign_match.group(1), known[assign_match.group(2)])
return (MoveGroupInfoLevel.SUCCESS, assign_match.group(1) + " is now the same as " + assign_match.group(2))
else:
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
# see if we have assignment of matlab-like vector syntax
set_match = re.match(r"^(\w+)\s*=\s*\[([\d\.e\-\+\s]+)\]$", cmd)
if set_match:
try:
g.remember_joint_values(set_match.group(1), [float(x) for x in set_match.group(2).split()])
return (MoveGroupInfoLevel.SUCCESS, "Remembered joint values [" + set_match.group(2) + "] under the name " + set_match.group(1))
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse joint value [" + set_match.group(2) + "]")
# see if we have assignment of matlab-like element update
component_match = re.match(r"^(\w+)\s*\[\s*(\d+)\s*\]\s*=\s*([\d\.e\-\+]+)$", cmd)
if component_match:
known = g.get_remembered_joint_values()
if known.has_key(component_match.group(1)):
try:
val = known[component_match.group(1)]
val[int(component_match.group(2))] = float(component_match.group(3))
g.remember_joint_values(component_match.group(1), val)
return (MoveGroupInfoLevel.SUCCESS, "Updated " + component_match.group(1) + "[" + component_match.group(2) + "]")
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse index or value in '" + cmd +"'")
else:
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
clist = cmdorig.split()
clist[0] = clist[0].lower()
# if this is an unknown one-word command, it is probably a variable
if len(clist) == 1:
known = g.get_remembered_joint_values()
if known.has_key(cmd):
return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[cmd]]) + "]")
else:
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
# command with one argument
if len(clist) == 2:
if clist[0] == "go":
self._last_plan = None
if clist[1] == "rand" or clist[1] == "random":
vals = g.get_random_joint_values()
g.set_joint_value_target(vals)
if g.go():
return (MoveGroupInfoLevel.SUCCESS, "Moved to random target [" + " ".join([str(x) for x in vals]) + "]")
else:
return (MoveGroupInfoLevel.FAIL, "Failed while moving to random target [" + " ".join([str(x) for x in vals]) + "]")
else:
try:
g.set_named_target(clist[1])
if g.go():
return (MoveGroupInfoLevel.SUCCESS, "Moved to " + clist[1])
else:
return (MoveGroupInfoLevel.FAIL, "Failed while moving to " + clist[1])
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, "Going to " + clist[1] + ": " + str(e))
except:
return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown")
if clist[0] == "plan":
self._last_plan = None
vals = None
if clist[1] == "rand" or clist[1] == "random":
vals = g.get_random_joint_values()
g.set_joint_value_target(vals)
self._last_plan = g.plan()
else:
try:
g.set_named_target(clist[1])
self._last_plan = g.plan()
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, "Planning to " + clist[1] + ": " + str(e))
except:
return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown")
if self._last_plan != None:
if len(self._last_plan.joint_trajectory.points) == 0 and len(self._last_plan.multi_dof_joint_trajectory.points) == 0:
self._last_plan = None
dest_str = clist[1]
if vals != None:
dest_str = "[" + " ".join([str(x) for x in vals]) + "]"
if self._last_plan != None:
return (MoveGroupInfoLevel.SUCCESS, "Planned to " + dest_str)
else:
return (MoveGroupInfoLevel.FAIL, "Failed while planning to " + dest_str)
elif clist[0] == "pick":
self._last_plan = None
if g.pick(clist[1]):
return (MoveGroupInfoLevel.SUCCESS, "Picked object " + clist[1])
else:
return (MoveGroupInfoLevel.FAIL, "Failed while trying to pick object " + clist[1])
elif clist[0] == "place":
self._last_plan = None
if g.place(clist[1]):
return (MoveGroupInfoLevel.SUCCESS, "Placed object " + clist[1])
else:
return (MoveGroupInfoLevel.FAIL, "Failed while trying to place object " + clist[1])
elif clist[0] == "planner":
g.set_planner_id(clist[1])
return (MoveGroupInfoLevel.SUCCESS, "Planner is now " + clist[1])
elif clist[0] == "record" or clist[0] == "rec":
g.remember_joint_values(clist[1])
return (MoveGroupInfoLevel.SUCCESS, "Remembered current joint values under the name " + clist[1])
elif clist[0] == "rand" or clist[0] == "random":
g.remember_joint_values(clist[1], g.get_random_joint_values())
return (MoveGroupInfoLevel.SUCCESS, "Remembered random joint values under the name " + clist[1])
elif clist[0] == "del" or clist[0] == "delete":
g.forget_joint_values(clist[1])
return (MoveGroupInfoLevel.SUCCESS, "Forgot joint values under the name " + clist[1])
elif clist[0] == "show":
known = g.get_remembered_joint_values()
if known.has_key(clist[1]):
return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[clist[1]]]) + "]")
else:
return (MoveGroupInfoLevel.WARN, "Joint values for " + clist[1] + " are not known")
elif clist[0] == "tol" or clist[0] == "tolerance":
try:
g.set_goal_tolerance(float(clist[1]))
return (MoveGroupInfoLevel.SUCCESS, "OK")
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse tolerance value '" + clist[1] + "'")
elif clist[0] == "time":
try:
g.set_planning_time(float(clist[1]))
return (MoveGroupInfoLevel.SUCCESS, "OK")
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse planning duration value '" + clist[1] + "'")
elif clist[0] == "constrain":
try:
g.set_path_constraints(clist[1])
return (MoveGroupInfoLevel.SUCCESS, "OK")
except:
if self._db_host != None:
return (MoveGroupInfoLevel.WARN, "Constraint " + clist[1] + " is not known.")
else:
return (MoveGroupInfoLevel.WARN, "Not connected to a database.")
elif clist[0] == "wait":
try:
time.sleep(float(clist[1]))
return (MoveGroupInfoLevel.SUCCESS, clist[1] + " seconds passed")
except:
return (MoveGroupInfoLevel.WARN, "Unable to wait '" + clist[1] + "' seconds")
elif clist[0] == "database":
try:
g.set_constraints_database(clist[1], self._db_port)
self._db_host = clist[1]
return (MoveGroupInfoLevel.SUCCESS, "Connected to " + self._db_host + ":" + str(self._db_port))
except:
return (MoveGroupInfoLevel.WARN, "Unable to connect to '" + clist[1] + ":" + str(self._db_port) + "'")
else:
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
if len(clist) == 3:
if clist[0] == "go" and self.GO_DIRS.has_key(clist[1]):
self._last_plan = None
try:
offset = float(clist[2])
(axis, factor) = self.GO_DIRS[clist[1]]
return self.command_go_offset(g, offset, factor, axis, clist[1])
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, "Going " + clist[1] + ": " + str(e))
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse distance '" + clist[2] + "'")
elif clist[0] == "allow" and (clist[1] == "look" or clist[1] == "looking"):
if clist[2] == "1" or clist[2] == "true" or clist[2] == "T" or clist[2] == "True":
g.allow_looking(True)
else:
g.allow_looking(False)
return (MoveGroupInfoLevel.DEBUG, "OK")
elif clist[0] == "allow" and (clist[1] == "replan" or clist[1] == "replanning"):
if clist[2] == "1" or clist[2] == "true" or clist[2] == "T" or clist[2] == "True":
g.allow_replanning(True)
else:
g.allow_replanning(False)
return (MoveGroupInfoLevel.DEBUG, "OK")
elif clist[0] == "database":
try:
g.set_constraints_database(clist[1], int(clist[2]))
self._db_host = clist[1]
self._db_port = int(clist[2])
return (MoveGroupInfoLevel.SUCCESS, "Connected to " + self._db_host + ":" + str(self._db_port))
except:
self._db_host = None
return (MoveGroupInfoLevel.WARN, "Unable to connect to '" + clist[1] + ":" + clist[2] + "'")
if len(clist) == 4:
if clist[0] == "rotate":
try:
g.set_rpy_target([float(x) for x in clist[1:]])
if g.go():
return (MoveGroupInfoLevel.SUCCESS, "Rotation complete")
else:
return (MoveGroupInfoLevel.FAIL, "Failed while rotating to " + " ".join(clist[1:]))
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, str(e))
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse X-Y-Z rotation values '" + " ".join(clist[1:]) + "'")
if len(clist) >= 7:
if clist[0] == "go":
self._last_plan = None
approx = False
if (len(clist) > 7):
if (clist[7] == "approx" or clist[7] == "approximate"):
approx = True
try:
p = Pose()
p.position.x = float(clist[1])
p.position.y = float(clist[2])
p.position.z = float(clist[3])
q = tf.transformations.quaternion_from_euler(float(clist[4]), float(clist[5]), float(clist[6]))
p.orientation.x = q[0]
p.orientation.y = q[1]
p.orientation.z = q[2]
p.orientation.w = q[3]
if approx:
g.set_joint_value_target(p, True)
else:
g.set_pose_target(p)
if g.go():
return (MoveGroupInfoLevel.SUCCESS, "Moved to pose target\n%s\n" % (str(p)))
else:
return (MoveGroupInfoLevel.FAIL, "Failed while moving to pose \n%s\n" % (str(p)))
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, "Going to pose target: %s" % (str(e)))
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse pose '" + " ".join(clist[1:]) + "'")
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
def command_show(self, g):
known = g.get_remembered_joint_values()
res = []
for k in known.keys():
res.append(k + " = [" + " ".join([str(x) for x in known[k]]) + "]")
return (MoveGroupInfoLevel.INFO, "\n".join(res))
def command_current(self, g):
res = "joints = [" + " ".join([str(x) for x in g.get_current_joint_values()]) + "]"
if len(g.get_end_effector_link()) > 0:
res = res + "\n" + g.get_end_effector_link() + " pose = [\n" + str(g.get_current_pose()) + " ]"
res = res + "\n" + g.get_end_effector_link() + " RPY = " + str(g.get_current_rpy())
return (MoveGroupInfoLevel.INFO, res)
def command_go_offset(self, g, offset, factor, dimension_index, direction_name):
if g.has_end_effector_link():
try:
g.shift_pose_target(dimension_index, factor * offset)
if g.go():
return (MoveGroupInfoLevel.SUCCESS, "Moved " + direction_name + " by " + str(offset) + " m")
else:
return (MoveGroupInfoLevel.FAIL, "Failed while moving " + direction_name)
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, str(e))
except:
return (MoveGroupInfoLevel.WARN, "Unable to process pose update")
else:
return (MoveGroupInfoLevel.WARN, "No known end effector. Cannot move " + direction_name)
def resolve_command_alias(self, cmdorig):
cmd = cmdorig.lower()
if cmd == "which":
return "id"
if cmd == "groups":
return "use"
return cmdorig
def get_help(self):
res = []
res.append("Known commands:")
res.append(" help show this screen")
res.append(" id|which display the name of the group that is operated on")
res.append(" load [<file>] load a set of interpreted commands from a file")
res.append(" save [<file>] save the currently known variables as a set of commands")
res.append(" use <name> switch to using the group named <name> (and load it if necessary)")
res.append(" use|groups show the group names that are already loaded")
res.append(" vars display the names of the known states")
res.append(" show display the names and values of the known states")
res.append(" show <name> display the value of a state")
res.append(" record <name> record the current joint values under the name <name>")
res.append(" delete <name> forget the joint values under the name <name>")
res.append(" current show the current state of the active group")
res.append(" constrain <name> use the constraint <name> as a path constraint")
res.append(" constrain clear path constraints")
res.append(" eef print the name of the end effector attached to the current group")
res.append(" go <name> plan and execute a motion to the state <name>")
res.append(" go <dir> <dx>| plan and execute a motion in direction up|down|left|right|forward|backward for distance <dx>")
res.append(" go rand plan and execute a motion to a random state")
res.append(" plan <name> plan a motion to the state <name>")
res.append(" execute execute a previously computed motion plan")
res.append(" rotate <x> <y> <z> plan and execute a motion to a specified orientation (about the X,Y,Z axes)")
res.append(" tolerance show the tolerance for reaching the goal region")
res.append(" tolerance <val> set the tolerance for reaching the goal region")
res.append(" wait <dt> sleep for <dt> seconds")
res.append(" x = y assign the value of y to x")
res.append(" x[idx] = val assign a value to dimension idx of x")
res.append(" x = [v1 v2...] assign a vector of values to x")
res.append(" trace <on|off> enable/disable replanning or looking around")
res.append(" ground add a ground plane to the planning scene")
res.append(" allow replanning <true|false> enable/disable replanning")
res.append(" allow looking <true|false> enable/disable looking around")
return "\n".join(res)
def get_keywords(self):
known_vars = []
known_constr = []
if self.get_active_group() != None:
known_vars = self.get_active_group().get_remembered_joint_values().keys()
known_constr = self.get_active_group().get_known_constraints()
groups = self._robot.get_group_names()
return {'go':['up', 'down', 'left', 'right', 'backward', 'forward', 'random'] + known_vars,
'help':[],
'record':known_vars,
'show':known_vars,
'wait':[],
'delete':known_vars,
'database': [],
'current':[],
'use':groups,
'load':[],
'save':[],
'pick':[],
'place':[],
'plan':known_vars,
'allow':['replanning', 'looking'],
'constrain':known_constr,
'vars':[],
'joints':[],
'tolerance':[],
'time':[],
'eef':[],
'execute':[],
'ground':[],
'id':[]}
class Arm():
default_planner = "BKPIECEkConfigDefault"
link_type_map = {
LinkType.INTERMEDIATE: IntermediateLink,
LinkType.TERMINAL: TerminalLink
}
def __init__(self,
planner=default_planner,
links={"ee": (LinkType.INTERMEDIATE, "manipulator")}):
# initialize interface node (if needed)
if rospy.get_node_uri() is None:
self.node_name = 'ur5_interface_node'
rospy.init_node(self.node_name)
ROS_INFO(
"The Arm interface was created outside a ROS node, a new node will be initialized!"
)
else:
ROS_INFO(
"The Arm interface was created within a ROS node, no need to create a new one!"
)
# store parameters
self._planner = planner
self._links = links
# create a RobotCommander
self._robot = RobotCommander()
# create semaphore
self._semaphore = BoundedSemaphore(value=1)
# default link (for utility functions only)
self._default_link = None
# create links objects
num_valid_links = 0
for link in self._links.items():
link_name, link_description = link
link_type, link_group_name = link_description
link_class = Arm.link_type_map[link_type]
link_obj = link_class(self, link_group_name, self._planner)
if not self._robot.has_group(link_group_name):
ROS_ERR("Move group `%s` not found!", link_group_name)
else:
self.__dict__[link_name] = link_obj
self._default_link = link_obj
num_valid_links += 1
if num_valid_links <= 0:
ROS_ERR("No valid links were found")
return None
# keep track of the status of the node
self._is_shutdown = False
self.verbose = False
def get_robot_state(self):
return self._robot.get_current_state()
def get_group_names(self):
return self._robot.get_group_names()
def get_planning_frame(self):
return self._default_link._group.get_planning_frame()
def get_joint_values(self):
return self._default_link._group.get_current_joint_values()
def plan_to_joint_config(self, joint_angles):
''' Plan to a given joint configuration '''
# Clear old pose targets
self._default_link._group.clear_pose_targets()
# Set Joint configuration target
self._default_link._group.set_joint_value_target(joint_angles)
numTries = 0
while numTries < 5:
success, plan, _, _ = self._default_link._group.plan()
numTries += 1
if success:
if self.verbose: print('succeeded in %d tries' % numTries)
return True, plan
if self.verbose: print("Planning failed")
return False, None
def execute_plan(self, plan):
''' Execute a given plan '''
self._semaphore.acquire()
out = self._default_link._group.execute(plan)
sleep(0.05)
self._semaphore.release()
return out
def _plan_to_pose(self, group, pose):
''' Plan to a given pose for the end-effector '''
# Clear old pose targets
group.clear_pose_targets()
constr = group.get_path_constraints()
if len(constr.orientation_constraints) > 0:
pose.orientation = constr.orientation_constraints[0].orientation
# Set target pose
group.set_pose_target(pose)
numTries = 0
while numTries < 5:
success, plan, _, _ = group.plan()
numTries += 1
# check that planning succeeded
if success:
if self.verbose: print('succeeded in %d tries' % numTries)
return True, plan
# if we get here, we couldn't find a plan in max number of tries
if self.verbose: print('Planning failed')
return False, None
def _plan_cartesian_path(self, group, waypoints):
numTries = 0
while numTries < 5:
success, plan, _, _ = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0 # jump_threshold
)
numTries += 1
# check that planning succeeded
if success:
if self.verbose: print('succeeded in %d tries' % numTries)
return True, plan
# if we get here, we couldn't find a plan in max number of tries
return False, None
def _execute_plan(self, group, plan):
''' Execute a given plan '''
self._semaphore.acquire()
out = group.execute(plan)
self._semaphore.release()
return out
def shutdown(self):
print('Shutting down Arm...')
self._is_shutdown = True
print('Arm released!')
return True
class Baxter(object):
def __init__(self):
# create a RobotCommander
self.robot = RobotCommander()
# create a PlanningScene Interface object
self.scene = PlanningSceneInterface()
# create left arm move group and gripper
self.left_arm = Arm("left_arm")
self.left_gripper = self.left_arm.gripper = Gripper("left")
# create right arm move group and gripper
self.right_arm = Arm("right_arm")
self.right_gripper = self.right_arm.gripper = Gripper("right")
self.gripper_offset = 0.0
# reset robot
self.reset()
def reset(self, ):
"""
Reset robot state and calibrate grippers
"""
# enable robot
self.enable()
# move to ready position
joint_angles = [0.0, -0.55, 0.0, 0.75, 0.0, 1.26, 0.0]
self.move_to_joints("right_arm", joint_angles, blocking=False)
self.move_to_joints("left_arm", joint_angles, blocking=True)
# calibrate grippers
self.calibrate_grippers()
return
def enable(self):
"""
Enable robot
"""
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
self._rs.enable()
def calibrate_grippers(self):
"""
Calibrate grippers
"""
if not self.left_gripper.calibrated():
self.left_gripper.calibrate()
if not self.right_gripper.calibrated():
self.right_gripper.calibrate()
return
def get_robot_state(self):
"""
Get robot state
Returns
ros msg (moveit_msgs.msg._RobotState.RobotState) - A message that contains
information about all of the joints, including fixed head pan,
revolute arm joints, and prismatic finger joints
"""
return self.robot.get_current_state()
def get_group_names(self):
"""
Get group names
Returns
group names (list): a list of move groups that can be used for planning
"""
return self.robot.get_group_names()
def pick(self):
return
def place(self):
return
p.pose.position.x = 0.7
p.pose.position.y = 0.7
p.pose.position.z = 0.0
# add the cup
scene.add_mesh("cup",p,resourcepath+'objects/cup.dae')
# modify the pose
p.pose.position.x = 0.72
p.pose.position.z = 0.05
# add the pen
scene.add_mesh("pen",p,resourcepath+'objects/pen.dae')
rospy.sleep(1)
# print the existing groups
robot = RobotCommander()
print "Available groups: ",robot.get_group_names()
# setup the arm group and its planner
arm = MoveGroupCommander("arm")
arm.set_start_state_to_current_state()
arm.set_planner_id("RRTstarkConfigDefault")
arm.set_planning_time(5.0)
arm.detach_object("pen")
# set the arm to a safe target
arm.set_named_target("gamma")
# plan and execute the motion
arm.go()
# setup the hand group and its planner
hand = MoveGroupCommander("hand")
p.pose.position.x = 0.7
p.pose.position.y = 0.7
p.pose.position.z = 0.0
# add the cup
scene.add_mesh("cup", p, resourcepath + 'objects/cup.dae')
# modify the pose
p.pose.position.x = 0.72
p.pose.position.z = 0.05
# add the pen
scene.add_mesh("pen", p, resourcepath + 'objects/pen.dae')
rospy.sleep(1)
# print the existing groups
robot = RobotCommander()
print "Available groups: ", robot.get_group_names()
# setup the arm group and its planner
arm = MoveGroupCommander("arm")
arm.set_start_state_to_current_state()
arm.set_planner_id("RRTstarkConfigDefault")
arm.set_planning_time(5.0)
arm.detach_object("pen")
# set the arm to a safe target
arm.set_named_target("gamma")
# plan and execute the motion
arm.go()
# setup the hand group and its planner
hand = MoveGroupCommander("hand")
move_group = MoveGroupCommander(group_name)
FIXED_FRAME = 'world'
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', DisplayTrajectory, queue_size=20)
# We can get the name of the reference frame for this robot:
planning_frame = move_group.get_planning_frame()
print "============ Planning frame: %s" % planning_frame
# We can also print the name of the end-effector link for this group:
eef_link = move_group.get_end_effector_link()
print "============ End effector link: %s" % eef_link
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print "============ Available Planning Groups:", robot.get_group_names()
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print "============ Printing robot state"
print robot.get_current_state()
print ""
# We can get the joint values from the group and adjust some of the values:
q1 = pi * 90 / 180
q2 = -pi * 130 / 180
q3 = pi * 111 / 180
q4 = -pi * 68 / 180
q5 = -pi * 90 / 180
q6 = 0
