def make_actions_executable(self):
assert len(openravepy.RaveGetEnvironments()) == 1
env = openravepy.RaveGetEnvironment(1)
for a in self.A:
if a.type == 'two_arm_pick' and type(
a.discrete_parameters['object']) == str:
a.discrete_parameters['object'] = env.GetKinBody(
a.discrete_parameters['object'])
def Restore(self):
assert len(openravepy.RaveGetEnvironments()) == 1
env = openravepy.RaveGetEnvironment(self.env_id)
robot = env.GetRobot(self.robot_name)
currently_holding = len(robot.GetGrabbed()) > 0
if currently_holding:
held_obj = robot.GetGrabbed()[0]
release_obj()
for obj_name, obj_pose in zip(self.object_poses.keys(), self.object_poses.values()):
set_obj_xyztheta(obj_pose, env.GetKinBody(obj_name))
set_robot_config(self.robot_base_pose, robot)
robot.SetDOFValues(self.robot_dof_values)
if self.is_holding:
held_obj = env.GetKinBody(self.held_object)
grab_obj(held_obj)
def Restore(self):
if self.env_id is None:
return
assert len(openravepy.RaveGetEnvironments()) == 1
env = openravepy.RaveGetEnvironment(self.env_id)
robot = env.GetRobot(self.robot_name)
currently_holding = len(robot.GetGrabbed()) > 0
if currently_holding:
held_obj = robot.GetGrabbed()[0]
release_obj(robot, held_obj)
for obj_name, obj_pose in zip(self.object_poses.keys(), self.object_poses.values()):
try:
set_obj_xytheta(obj_pose, env.GetKinBody(obj_name))
except:
print "Object ", obj_name, 'does not exist in the environment'
continue
set_robot_config(self.robot_base_pose, robot)
robot.SetDOFValues(self.robot_dof_values)
if self.is_holding:
held_obj = env.GetKinBody(self.held_object)
grab_obj(robot, held_obj)
def main():
### Parameters ###
ENV_FILE = "../../trajopt/data/pr2_table.env.xml"
MANIP_NAME = "rightarm"
N_STEPS = 10
LINK_NAME = "r_gripper_tool_frame"
INTERACTIVE = True
#joints_start_end = np.array([
# [-0.95, -0.38918253, -2.43888696, -1.23400121, -0.87433154, -0.97616443, -2.10997203],
# [-0.4, -0.4087081, -3.77121706, -1.2273375, 0.69885101, -0.8992004, 3.13313843]
#])
#joints_start_end = np.array([[0.34066373, -0.49439586, -3.3 , -1.31059503 , -1.28229698, -0.15682819, -116.19626995],
# [ 0.5162424 , -0.42037121 , -3.7 , -1.30277208 , 1.31120586, -0.16411924 ,-118.57637204]])
#joints_start_end = np.array([[ -1.83204054 , -0.33201855 , -1.01105089 , -1.43693186 , -1.099908, -2.00040616, -116.17133393],
#[ -0.38176851 , 0.17886005 , -1.4 , -1.89752658 , -1.93285873, -1.60546868, -114.70809047]])
joints_start_end = np.array([[
0.33487707, -0.50480484, -3.3, -1.33546928, -1.37194549, -0.14645853,
-116.11672039
],
[
4.71340480e-01, -4.56593341e-01,
-3.60000000e+00, -1.33176173e+00,
1.21859723e+00, -9.98780266e-02,
-1.18561732e+02
]])
##################
joints_start_end[:, 2] = np.unwrap(joints_start_end[:, 2])
joints_start_end[:, 4] = np.unwrap(joints_start_end[:, 4])
joints_start_end[:, 6] = np.unwrap(joints_start_end[:, 6])
joints_start = joints_start_end[0, :]
joints_end = joints_start_end[1, :]
### Env setup ####
env = rave.RaveGetEnvironment(1)
if env is None:
env = rave.Environment()
env.StopSimulation()
atexit.register(rave.RaveDestroy)
env.Load(ENV_FILE)
robot = env.GetRobots()[0]
manip = robot.GetManipulator(MANIP_NAME)
robot.SetDOFValues(joints_start, manip.GetArmIndices())
##################
result_traj = move_arm_straight(env,
manip,
N_STEPS,
LINK_NAME,
joints_start,
joints_end,
interactive=INTERACTIVE)
print 'Showing original straight-line trajectory'
env.SetViewer('qtcoin')
env.UpdatePublishedBodies()
import time
time.sleep(2)
play_traj(mu.linspace2d(joints_start, joints_end, N_STEPS), env, manip)
raw_input('press enter to continue')
print 'Showing optimized trajectory'
play_traj(result_traj, env, manip)
raw_input('press enter to continue')
return request
if __name__ == "__main__":
### Parameters ###
ENV_FILE = "wamtest1.env.xml"
MANIP_NAME = "arm"
LINK_NAME = "wam7"
#INTERACTIVE = True
##################
### Env setup ####
env = rave.RaveGetEnvironment(1)
if env is None:
env = rave.Environment()
env.StopSimulation()
env.Load(ENV_FILE)
robot = env.GetRobots()[0]
manip = robot.GetManipulator(MANIP_NAME)
##################
T_w_mug = env.GetKinBody("mug-table-cluttered").GetLinks()[0].GetTransform()
xyz_targ = (T_w_mug[:3,3] + np.array([0,0,.15])).tolist()
quat_targ = np.array([0,1,0,0]).tolist()
request = move_arm_to_grasp(xyz_targ, quat_targ, LINK_NAME, MANIP_NAME)
s = json.dumps(request)
print "REQUEST:",s
#trajoptpy.SetInteractive(False);
def __init__(self,
envid,
fixed_frame='base_link',
rate=10.,
logger=TextColors()):
"""
C{RaveStateUpdater} constructor. It uses a C{tf.TransformListener} instance to query TF.
By default, after creating it will start updating the environment at the given
rate. Every time it updates, check for new frame ids in TF.
The body names and the TF frame ids must have similarities and when possible
should be the same. We check C{frame_id in body_name} and C{body_namein frame_id}
this way if C{frame_id='left/base_link'} and C{body_name='denso_left'}, we will
split the C{frame_id} into C{['left','base_link']} and will be able to match
them.
@type envid: int
@param envid: Id of the OpenRAVE environment to be updated
@type fixed_frame: string
@param fixed_frame: Name of the TF frame to be considered as the fixed frame.It is also used to locate the corresponding
OpenRAVE body name.
@type rate: float
@param rate: Rate at which we will query TF for new transforms.
@type logger: Object
@param logger: Logger instance. When used in ROS, the recommended C{logger=rospy}.
"""
self.env = orpy.RaveGetEnvironment(envid)
try:
self.listener = tf.TransformListener()
except rospy.ROSInitException:
logger.logerr(
'time is not initialized. Have you called rospy.init_node()?')
return
# TODO: Wait until we hear something from TF
rospy.sleep(5.0)
# Subscribe to the joint_states topics that match a robot in OpenRAVE
topics = rosgraph.Master('/rostopic').getPublishedTopics('/')
self.js_topics = []
self.js_robots = []
for topic_info in topics:
topic_name = topic_info[0]
msg_class = roslib.message.get_message_class(topic_info[1])
if ('joint_states' in topic_name) and (msg_class is JointState):
namespace = topic_name.replace('/',
'').replace('joint_states', '')
robot = self._find_rave_body(namespace)
if robot is not None:
self.js_topics.append(topic_name)
self.js_robots.append(robot.GetName())
self.js_msgs = [None] * len(self.js_topics)
for topic in self.js_topics:
rospy.Subscriber(topic,
JointState,
self._cb_joint_states,
callback_args=topic)
# Look for the body that corresponds to the reference frame
self.fixed_frame = self._get_tf_reference_frame(fixed_frame)
if self.fixed_frame is None:
logger.logerr(
'Failed to find an unique frame in the TF tree. Found frames: {0}'
.format(self.listener.getFrameStrings()))
return
# Make the origin coincide with the reference frame body
self.ref_body = self._find_rave_body(self.fixed_frame)
if self.ref_body is None:
body_names = [body.GetName() for body in self.env.GetBodies()]
logger.logerr(
'Failed to find the reference body in OpenRAVE. Found bodies: {0}'
.format(body_names))
return
criros.raveutils.move_origin_to_body(self.ref_body)
# Create a publisher that will report the affected OpenRAVE objects
self.pub = rospy.Publisher('/openrave/state_updater',
String,
queue_size=3)
# Start a thread to update all the objects found in TF and OpenRAVE
self.rate = rate
self.elapsed_time = 0.0
self.update_rave_environment() # Update env at least once
self.timer = rospy.Timer(rospy.Duration(1.0 / rate),
self.update_rave_environment)
rospy.on_shutdown(self.stop)
def prm_connect(q1,
q2,
collision_checker,
source='',
return_start_set_and_path_idxs=False):
global prm_vertices
global prm_edges
is_goal_region = False
is_multiple_goals = False
if type(q2) is AARegion:
is_goal_region = True
else:
is_multiple_goals = isinstance(q2, list) # and len(q2[0]) == len(q1)
is_single_goal = not is_goal_region and not is_multiple_goals
if prm_vertices is None or prm_edges is None:
prm_vertices, prm_edges = pickle.load(open('./prm.pkl', 'rb'))
collision_checker_is_set = isinstance(collision_checker, set)
no_collision_checking = collision_checker_is_set and len(
collision_checker) == 0
## Base case checks
if not no_collision_checking:
env = openravepy.RaveGetEnvironment(1)
robot = env.GetRobot('pr2')
non_prm_config_collision_checker = collision_fn(env, robot)
if non_prm_config_collision_checker(q1):
print "Collision at init config"
if return_start_set_and_path_idxs:
return None, None
else:
return None
if is_single_goal and non_prm_config_collision_checker(q2):
print "Collision at goal config"
if return_start_set_and_path_idxs:
return None, None
else:
return None
if is_multiple_goals:
q2_original = q2
q2 = [
q_goal for q_goal in q2_original
if not non_prm_config_collision_checker(q_goal)
]
if len(q2) == 0:
if return_start_set_and_path_idxs:
return None, None
else:
return None
if is_single_goal and are_base_confs_close_enough(
q1, q2, xy_threshold=0.8, th_threshold=50.):
if return_start_set_and_path_idxs:
return [q1, q2], None
else:
return [q1, q2]
if is_multiple_goals:
for q_goal in q2:
if are_base_confs_close_enough(q1,
q_goal,
xy_threshold=0.8,
th_threshold=50.):
if return_start_set_and_path_idxs:
return [q1, q_goal], None
else:
return [q1, q_goal]
###
## Defines a goal test function
if is_multiple_goals:
close_points = []
def is_connected_to_goal(prm_vertex_idx):
q = prm_vertices[prm_vertex_idx]
goals = q2
if len(q.squeeze()) != 3:
raise NotImplementedError
# if source == 'sampler':
# diff = utils.base_conf_diff(q, q2[0])
# xydist = np.linalg.norm(diff[0:2])
# if xydist < 0.8:
# close_points.append(q)
return any(
are_base_confs_close_enough(
q, g, xy_threshold=0.8, th_threshold=360.) for g in goals)
elif is_goal_region:
def is_connected_to_goal(prm_vertex_idx):
q = prm_vertices[prm_vertex_idx]
goal_region = q2
return goal_region.contains_point(q)
else:
def is_connected_to_goal(prm_vertex_idx):
q = prm_vertices[prm_vertex_idx]
return are_base_confs_close_enough(q,
q2,
xy_threshold=0.8,
th_threshold=52.)
#####
def heuristic(q):
return 0
def is_collision(q):
col = q in collision_checker if collision_checker_is_set else non_prm_config_collision_checker(
prm_vertices[q])
return col
### making a set of qinit idxs
start = set()
for idx, q in enumerate(prm_vertices):
q_close_enough_to_q1 = are_base_confs_close_enough(q,
q1,
xy_threshold=0.8,
th_threshold=360.)
if q_close_enough_to_q1:
if not is_collision(idx):
start.add(idx)
#####
# todo can I get this set start? It can optimize v_manip computation
path_idxs = find_prm_path(start, [is_connected_to_goal], heuristic,
is_collision, source)[0]
# distances = [utils.base_pose_distance(q2[0], prm_vtx) for prm_vtx in prm_vertices]
# if source == 'sampler':
# import pdb;pdb.set_trace()
if path_idxs is not None:
path = [q1] + [prm_vertices[i] for i in path_idxs]
if is_single_goal:
path += [q2]
elif is_multiple_goals:
path += [get_goal_config_used(path, q2)]
if return_start_set_and_path_idxs:
return path, (start, path_idxs)
else:
return path
else:
if return_start_set_and_path_idxs:
return None, (start, path_idxs)
else:
return None
def prm_connect(q1, q2, collision_checker):
global prm_vertices
global prm_edges
is_goal_region = False
is_multiple_goals = False
if type(q2) is AARegion:
is_goal_region = True
else:
is_multiple_goals = isinstance(q2, list) # and len(q2[0]) == len(q1)
is_single_goal = not is_goal_region and not is_multiple_goals
collision_checker_is_set = isinstance(collision_checker, set)
if prm_vertices is None or prm_edges is None:
prm_vertices, prm_edges = pickle.load(open('./prm.pkl', 'rb'))
no_collision_checking = collision_checker_is_set and len(
collision_checker) == 0
if not no_collision_checking:
env = openravepy.RaveGetEnvironment(1)
robot = env.GetRobot('pr2')
non_prm_config_collision_checker = collision_fn(env, robot)
if non_prm_config_collision_checker(q1):
#print "initial config in collision"
return None
if is_single_goal and non_prm_config_collision_checker(q2):
#print "goal config in collision"
return None
if is_multiple_goals:
q2_original = q2
q2 = [
q_goal for q_goal in q2_original
if not non_prm_config_collision_checker(q_goal)
]
if len(q2) == 0:
return None
if is_single_goal and are_base_confs_close_enough(
q1, q2, xy_threshold=0.8, th_threshold=50.):
return [q1, q2]
if is_multiple_goals:
for q_goal in q2:
if are_base_confs_close_enough(q1,
q_goal,
xy_threshold=0.8,
th_threshold=50.):
return [q1, q_goal]
if is_multiple_goals:
def is_connected_to_goal(prm_vertex_idx):
q = prm_vertices[prm_vertex_idx]
goals = q2
if len(q.squeeze()) != 3:
raise NotImplementedError
return any(
are_base_confs_close_enough(
q, g, xy_threshold=0.8, th_threshold=50.) for g in goals)
elif is_goal_region:
def is_connected_to_goal(prm_vertex_idx):
q = prm_vertices[prm_vertex_idx]
goal_region = q2
return goal_region.contains_point(q)
else:
def is_connected_to_goal(prm_vertex_idx):
q = prm_vertices[prm_vertex_idx]
return are_base_confs_close_enough(q,
q2,
xy_threshold=0.8,
th_threshold=50.)
def heuristic(q):
return 0
def is_collision(q):
return q in collision_checker if collision_checker_is_set else collision_checker(
prm_vertices[q])
# start = {i for i, q in enumerate(prm_vertices) if np.linalg.norm((q - q1)[:2]) < .8}
start = set()
for idx, q in enumerate(prm_vertices):
q_close_enough_to_q1 = are_base_confs_close_enough(q,
q1,
xy_threshold=0.8,
th_threshold=50.)
if q_close_enough_to_q1:
if not is_collision(idx):
start.add(idx)
path = find_prm_path(start, [is_connected_to_goal], heuristic,
is_collision)[0]
if path is not None:
path = [q1] + [prm_vertices[i] for i in path]
if is_single_goal:
path += [q2]
elif is_multiple_goals:
path += [get_goal_config_used(path, q2)]
return path
else:
return None
