def visualize_path(path):
assert len(openravepy.RaveGetEnvironments()) == 1
env = openravepy.RaveGetEnvironments()[0]
robot = env.GetRobot('pr2')
dim_path = len(path[0])
if dim_path == 3:
robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1])
elif dim_path == 11:
manip = robot.GetManipulator('rightarm_torso')
robot.SetActiveDOFs(manip.GetArmIndices(), DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1])
else:
assert path[0].shape[0] == robot.GetActiveDOF(), 'robot and path should have same dof'
env = robot.GetEnv()
if len(path) > 1000:
path_reduced = path[0:len(path) - 1:int(len(path) * 0.1)]
else:
path_reduced = path
for idx, conf in enumerate(path_reduced):
is_goal_config = idx == len(path_reduced) - 1
if is_goal_config:
draw_robot_at_conf(conf, 0.5, 'path' + str(idx), robot, env)
else:
draw_robot_at_conf(conf, 0.9, 'path' + str(idx), robot, env)
raw_input("Continue?")
remove_drawn_configs('path', env)
def remove_drawn_configs(name, env=None):
if env is None:
assert len(openravepy.RaveGetEnvironments()) == 1
env = openravepy.RaveGetEnvironments()[0]
for body in env.GetBodies():
if body.GetName().find(name) != -1:
env.Remove(body)
def one_arm_place_object(place_action):
assert len(openravepy.RaveGetEnvironments()) == 1
env = openravepy.RaveGetEnvironments()[0]
robot = env.GetRobot('pr2')
manip = robot.GetManipulator('rightarm_torso')
robot.SetActiveDOFs(manip.GetArmIndices(), DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1])
place_config = place_action['q_goal']
set_active_config(place_config)
release_obj()
open_gripper(robot)
def visualize_placements(placements, obj=None):
assert len(openravepy.RaveGetEnvironments()) == 1
env = openravepy.RaveGetEnvironments()[0]
if obj is None:
obj = env.GetRobots()[0].GetGrabbed()[0]
if type(obj) == unicode or type(obj) == str:
obj = env.GetKinBody(obj)
for idx, conf in enumerate(placements):
draw_obj_at_conf(conf, 0.7, 'place' + str(idx), obj, env)
raw_input("Continue?")
remove_drawn_configs('place', env)
def fold_arms():
assert len(openravepy.RaveGetEnvironments()) == 1
env = openravepy.RaveGetEnvironments()[0]
robot = env.GetRobot('pr2')
leftarm_manip = robot.GetManipulator('leftarm')
rightarm_manip = robot.GetManipulator('rightarm')
FOLDED_LEFT_ARM = [0.0, 1.29023451, 0.0, -2.12, 0.0, -0.69800004, 0.0]
set_config(robot, FOLDED_LEFT_ARM, leftarm_manip.GetArmIndices())
set_config(robot, mirror_arm_config(FOLDED_LEFT_ARM), rightarm_manip.GetArmIndices())
def set_body_transparency(body, transparency):
env = openravepy.RaveGetEnvironments()[0]
if type(body) == unicode or type(body) == str:
body = env.GetKinBody(body)
for link in body.GetLinks():
for geom in link.GetGeometries():
geom.SetTransparency(transparency)
def set_robot_config(base_pose, robot=None):
if robot is None:
env = openravepy.RaveGetEnvironments()[0]
robot = env.GetRobot('pr2')
base_pose = np.array(base_pose)
base_pose = clean_pose_data(base_pose.astype('float'))
robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1])
base_pose = np.array(base_pose).squeeze()
"""
while base_pose[-1] < 0:
try:
factor = -int(base_pose[-1] /(2*np.pi))
except:
import pdb;pdb.set_trace()
if factor == 0: factor = 1
base_pose[-1] += factor*2*np.pi
while base_pose[-1] > 2*np.pi:
factor = int(base_pose[-1] /(2*np.pi))
base_pose[-1] -= factor*2*np.pi
if base_pose[-1] <
if base_pose[-1] > 1.01:
base_pose[-1] = 1.01
elif base_pose[-1] < 0.99:
base_pose[-1] = 0.99
"""
# print base_pose
robot.SetActiveDOFValues(base_pose)
def get_body_xytheta(body):
if not isinstance(body, openravepy.KinBody):
env = openravepy.RaveGetEnvironments()[0]
body = env.GetKinBody(body)
Tbefore = body.GetTransform()
body_quat = get_quat(body)
th1 = np.arccos(body_quat[0]) * 2
th2 = np.arccos(-body_quat[0]) * 2
th3 = -np.arccos(body_quat[0]) * 2
quat_th1 = quat_from_angle_vector(th1, np.array([0, 0, 1]))
quat_th2 = quat_from_angle_vector(th2, np.array([0, 0, 1]))
quat_th3 = quat_from_angle_vector(th3, np.array([0, 0, 1]))
if np.all(np.isclose(body_quat, quat_th1)):
th = th1
elif np.all(np.isclose(body_quat, quat_th2)):
th = th2
elif np.all(np.isclose(body_quat, quat_th3)):
th = th3
else:
print "This should not happen. Check if object is not standing still"
import pdb;
pdb.set_trace()
if th < 0: th += 2 * np.pi
assert (0 <= th < 2 * np.pi)
# set the quaternion using the one found
set_quat(body, quat_from_angle_vector(th, np.array([0, 0, 1])))
Tafter = body.GetTransform()
assert (np.all(np.isclose(Tbefore, Tafter)))
body_xytheta = np.hstack([get_point(body)[0:2], th])
body_xytheta = body_xytheta[None, :]
clean_pose_data(body_xytheta)
return body_xytheta
def randomly_place_region(body, region, n_limit=None):
env = openravepy.RaveGetEnvironments()[0]
if env.GetKinBody(get_name(body)) is None:
env.Add(body)
orig = get_body_xytheta(body)
# for _ in n_limit:
i = 0
while True:
set_quat(body, quat_from_z_rot(uniform(0, 2 * PI)))
aabb = aabb_from_body(body)
cspace = region.cspace(aabb)
if cspace is None: continue
set_point(
body,
np.array([uniform(*range) for range in cspace] +
[region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET])
- aabb.pos() + get_point(body))
if not body_collision(env, body):
return
if n_limit is not None:
i += 1
if i >= n_limit:
set_obj_xytheta(orig, body)
return
def two_arm_place_object(place_action):
assert len(openravepy.RaveGetEnvironments()) == 1
env = openravepy.RaveGetEnvironments()[0]
robot = env.GetRobot('pr2')
try:
place_base_pose = place_action['q_goal']
except KeyError:
place_base_pose = place_action['base_pose']
leftarm_manip = robot.GetManipulator('leftarm')
rightarm_manip = robot.GetManipulator('rightarm')
set_robot_config(place_base_pose, robot)
release_obj()
set_config(robot, FOLDED_LEFT_ARM, leftarm_manip.GetArmIndices())
set_config(robot, mirror_arm_config(FOLDED_LEFT_ARM), rightarm_manip.GetArmIndices())
def convert_to_kin_body(obj):
env = openravepy.RaveGetEnvironments()[0]
if isinstance(obj, openravepy.KinBody):
obj = obj
else:
obj = env.GetKinBody(obj)
return obj
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 get_color_of(body):
env = openravepy.RaveGetEnvironments()[0]
if type(body) == unicode or type(body) == str:
obj = env.GetKinBody(body)
else:
obj = body
return get_color(obj)
def get_global_pose_from_relative_pose_to_body(body, rel_robot_pose):
if not isinstance(body, openravepy.KinBody):
env = openravepy.RaveGetEnvironments()[0]
body = env.GetKinBody(body)
t_body = body.GetTransform()
t_pose = get_transform_from_pose(rel_robot_pose, 'robot')
t_pose_global = get_xytheta_from_transform(np.dot(t_body, t_pose))
return t_pose_global
def visualize_values_in_two_arm_domains(self, entity_values, entity_names):
is_place_node = entity_names[0].find('region') != -1
if is_place_node:
return
max_val = np.max(entity_values)
min_val = np.min(entity_values)
for entity_name, entity_value in zip(entity_names, entity_values):
entity = openravepy.RaveGetEnvironments()[0].GetKinBody(entity_name)
set_color(entity, [0, (entity_value - min_val) / (max_val - min_val), 0])
def compute_occ_vec(key_configs):
env = openravepy.RaveGetEnvironments()[0]
robot = env.GetRobot('pr2')
occ_vec = []
with robot:
for config in key_configs:
set_robot_config(config, robot)
collision = env.CheckCollision(robot) * 1
occ_vec.append(collision)
return np.array(occ_vec)
def set_obj_xytheta(xytheta, obj):
if isinstance(xytheta, list) or isinstance(xytheta, tuple):
xytheta = np.array(xytheta)
env = openravepy.RaveGetEnvironments()[0]
if type(obj) == unicode or type(obj) == str:
obj = env.GetKinBody(obj)
xytheta = xytheta.squeeze()
set_quat(obj, quat_from_angle_vector(xytheta[-1], np.array([0, 0, 1])))
set_xy(obj, xytheta[0], xytheta[1])
def set_rightarm_torso(config, robot=None):
if robot is None:
env = openravepy.RaveGetEnvironments()[0]
robot = env.GetRobot('pr2')
manip = robot.GetManipulator('rightarm_torso')
robot.SetActiveDOFs(manip.GetArmIndices(),
DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis,
[0, 0, 1])
set_active_config(config, robot)
def visualize_pick_and_place(pick, place):
env = openravepy.RaveGetEnvironments()[0]
obj = pick.discrete_parameters['object']
if type(obj) == unicode or type(object) == str:
obj = env.GetKinBody(obj)
saver = CustomStateSaver(env)
visualize_path(pick.low_level_motion)
two_arm_pick_object(obj, pick.continuous_parameters)
visualize_path(place.low_level_motion)
saver.Restore()
def two_arm_pick_object(obj, pick_action):
assert len(openravepy.RaveGetEnvironments()) == 1
env = openravepy.RaveGetEnvironments()[0]
robot = env.GetRobot('pr2')
if type(obj) == str or type(obj) == unicode:
obj = robot.GetEnv().GetKinBody(obj)
try:
base_pose = pick_action['q_goal']
except KeyError:
base_pose = pick_action['base_pose']
set_robot_config(base_pose, robot)
if 'g_config' in pick_action.keys():
g_config = pick_action['g_config']
leftarm_manip = robot.GetManipulator('leftarm')
rightarm_torso_manip = robot.GetManipulator('rightarm_torso')
set_config(robot, g_config[0], leftarm_manip.GetArmIndices())
set_config(robot, g_config[1], rightarm_torso_manip.GetArmIndices())
grab_obj(obj)
def get_robot_xytheta(robot=None):
if robot is None:
env = openravepy.RaveGetEnvironments()[0]
robot = env.GetRobot('pr2')
with robot:
robot.SetActiveDOFs([],
DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis,
[0, 0, 1])
robot_xytheta = robot.GetActiveDOFValues()
robot_xytheta = robot_xytheta[None, :]
clean_pose_data(robot_xytheta)
return robot_xytheta
def randomly_place_region(body, region):
env = openravepy.RaveGetEnvironments()[0]
if env.GetKinBody(get_name(body)) is None:
env.Add(body)
while True:
set_quat(body, quat_from_z_rot(uniform(0, 2 * PI)))
aabb = aabb_from_body(body)
cspace = region.cspace(aabb)
if cspace is None: continue
set_point(body, np.array([uniform(*range) for range in cspace] + [
region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET]) - aabb.pos() + get_point(body))
if not body_collision(env, body):
return
def get_pick_base_pose_and_grasp_from_pick_parameters(obj,
pick_parameters,
obj_xyth=None):
if not isinstance(obj, openravepy.KinBody):
env = openravepy.RaveGetEnvironments()[0]
obj = env.GetKinBody(obj)
assert len(pick_parameters) == 6
grasp_params = pick_parameters[0:3]
ir_params = pick_parameters[3:]
pick_base_pose = get_absolute_pick_base_pose_from_ir_parameters(
ir_params, obj, obj_xyth)
return grasp_params, pick_base_pose
def execute_pick(self):
env = openravepy.RaveGetEnvironments()[0]
if isinstance(self.discrete_parameters['object'], openravepy.KinBody):
obj_to_pick = self.discrete_parameters['object']
else:
obj_to_pick = env.GetKinBody(self.discrete_parameters['object'])
if self.type == 'two_arm_pick_two_arm_place':
two_arm_pick_object(obj_to_pick,
self.continuous_parameters['pick'])
else:
one_arm_pick_object(obj_to_pick,
self.continuous_parameters['pick'])
def get_rightarm_torso_config(robot=None):
if robot is None:
env = openravepy.RaveGetEnvironments()[0]
robot = env.GetRobot('pr2')
with robot:
manip = robot.GetManipulator('rightarm_torso')
robot.SetActiveDOFs(manip.GetArmIndices(),
DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis,
[0, 0, 1])
robot_config = robot.GetActiveDOFValues()
robot_config = robot_config[None, :]
clean_rightarm_torso_base_pose(robot_config[0, :])
return robot_config
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 execute(self):
env = openravepy.RaveGetEnvironments()[0]
if self.type == 'two_arm_pick':
obj_to_pick = utils.convert_to_kin_body(
self.discrete_parameters['object'])
if 'q_goal' in self.continuous_parameters and type(self.continuous_parameters['q_goal']) == list and\
len(self.continuous_parameters['q_goal']) > 1:
try:
two_arm_pick_object(
obj_to_pick,
{'q_goal': self.continuous_parameters['q_goal'][0]})
except:
import pdb
pdb.set_trace()
else:
two_arm_pick_object(obj_to_pick, self.continuous_parameters)
elif self.type == 'two_arm_place':
two_arm_place_object(self.continuous_parameters)
elif self.type == 'two_arm_pick_two_arm_place':
obj_to_pick = utils.convert_to_kin_body(
self.discrete_parameters['object'])
two_arm_pick_object(obj_to_pick,
self.continuous_parameters['pick'])
two_arm_place_object(self.continuous_parameters['place'])
elif self.type == 'one_arm_pick':
obj_to_pick = utils.convert_to_kin_body(
self.discrete_parameters['object'])
one_arm_pick_object(obj_to_pick, self.continuous_parameters)
elif self.type == 'one_arm_place':
one_arm_place_object(self.continuous_parameters)
elif self.type == 'one_arm_pick_one_arm_place':
obj_to_pick = utils.convert_to_kin_body(
self.discrete_parameters['object'])
one_arm_pick_object(obj_to_pick,
self.continuous_parameters['pick'])
one_arm_place_object(self.continuous_parameters['place'])
else:
raise NotImplementedError
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 get_body_with_name(obj_name):
env = openravepy.RaveGetEnvironments()[0]
return env.GetKinBody(obj_name)
def set_active_config(conf, robot=None):
if robot is None:
env = openravepy.RaveGetEnvironments()[0]
robot = env.GetRobot('pr2')
robot.SetActiveDOFValues(conf.squeeze())
