def rotate_obj(): #### rotate obj by theta theta=PI/4 obj.SetTransform(np.eye(4)) rot = quat_from_angle_vector( theta,np.array([0,0,1])) set_quat(obj,rot) o_wrt_w = obj.GetTransform()
def randomly_place_in_region_need_to_be_fixed(env, obj, region, th=None):
# todo fix this function
min_x = region.box[0][0]
max_x = region.box[0][1]
min_y = region.box[1][0]
max_y = region.box[1][1]
aabb = aabb_from_body(obj)
# try 1000 placements
for _ in range(300):
x = np.random.rand(1) * (max_x - min_x) + min_x
y = np.random.rand(1) * (max_y - min_y) + min_y
z = [region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET]
xyz = np.array([x[0],y[0],z[0]]) - aabb.pos() + get_point(obj) # todo: recheck this function; I think it failed if obj was robot
if th == None:
th = np.random.rand(1) * 2 * np.pi
set_point(obj, xyz)
set_quat(obj, quat_from_angle_vector(th, np.array([0, 0, 1])))
obj_quat = get_quat(obj)
# refer to conversion between quaternions and euler angles on Wiki for the following eqn.
assert (np.isclose(th, np.arccos(obj_quat[0]) * 2)
or np.isclose(th, np.arccos(-obj_quat[0]) * 2))
# print not env.CheckCollision(obj) and region.contains(obj.ComputeAABB())
if not env.CheckCollision(obj) \
and region.contains(obj.ComputeAABB()):
return [x[0], y[0], th[0]]
return None
def randomly_place_in_region(env, body, region):
if env.GetKinBody(get_name(body)) is None: env.Add(body)
for i in range(1000):
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(*cspace_range) for cspace_range in cspace] + [
region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET]) - aabb.pos() + get_point(body))
if not env.CheckCollision(body):
return get_body_xytheta(body)
return None
def load_objects(env, obj_shapes, obj_poses, color):
objects = []
i = 0
nobj = len(obj_shapes.keys())
for obj_name in obj_shapes.keys():
xytheta = obj_poses[obj_name].squeeze()
width, length, height = obj_shapes[obj_name]
quat = quat_from_z_rot(xytheta[-1])
new_body = box_body(env, width, length, height, \
name=obj_name, \
color=np.array(color) / float(nobj - i))
i += 1
env.Add(new_body)
set_point(new_body, [xytheta[0], xytheta[1], 0.075])
set_quat(new_body, quat)
objects.append(new_body)
return objects
def gaussian_randomly_place_in_region(env, body, region, center, var):
if env.GetKinBody(get_name(body)) is None:
env.Add(body)
for i in range(1000):
xytheta = np.random.normal(center, var)
set_obj_xytheta(xytheta, body)
if not body_collision(env, body):
return xytheta
import pdb;pdb.set_trace()
for i in range(1000):
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(*cspace_range) for cspace_range in cspace] + [
region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET]) - aabb.pos() + get_point(body))
if not body_collision(env, body):
return get_body_xytheta(body)
return None
def compute_grasp(pitch,z_rotation,z_portion,slide_portion,approach_portion,obj):
# returns tool transform given grasp parameters, given a
# box-shaped obj
### Notations
# w = world
# o = obj
# ee = end-effector
# Place object at some location
set_quat(obj,quat_from_z_rot(PI/2))
set_point(obj,np.array([-0.8,0.188,1.01967739]))
o_wrt_w = copy.deepcopy(get_trans(obj))
### First, compute the gtrans, the relative trans of tool wrt obj,
### by temporarily moving object to the origin
# place it origin for convenience for computing rotation
obj.SetTransform(np.eye(4))
aabb = obj.ComputeAABB()
x_extent = aabb.extents()[0]
y_extent = aabb.extents()[1]
z_extent = aabb.extents()[2]
yaws = [0,PI/2,PI,3*PI/2] # rotation about z axis
for yaw in yaws:
tool_point = aabb.pos() - np.array([0,0,z_extent]) + \
np.array([0,0,z_portion*2*z_extent])
tool_point = tool_point - np.array([0,y_extent,0]) + \
np.array([0,slide_portion*2*y_extent,0])
tool_point = tool_point - np.array([x_extent,0,0]) + \
np.array([2*x_extent*approach_portion,0,0])
# compute the desired rotation of tool transform wrt world frame
desired_pitch = quat_from_angle_vector(pitch,np.array([0,1,0]))
desired_yaw = quat_from_angle_vector(yaw,np.array([0,0,1]))
# order of rotation matters; TODO study this later
tool_rot_wrt_w = quat_dot(desired_yaw,desired_pitch)
desired_tool_wrt_w = trans_from_quat_point(tool_rot_wrt_w, tool_point )
# Compute the tool transform wrt object frame. We call this grasp transform.
gtrans = compute_tool_trans_wrt_obj_trans(desired_tool_wrt_w, get_trans(obj))
# Compute the corresponding ee transform corresponding to the desired tool transform
# Good for visualization of where the grasp will be
desired_ee_world = compute_Tee_at_given_Ttool(desired_tool_wrt_w, \
manip.GetLocalToolTransform())
ee.SetTransform(desired_ee_world)
import pdb;pdb.set_trace()
##### End of computing relative transform of tool
### Now move object back to the original location, and then
### use the relative tool trans, gtrans, to compute a new grasp
# move object back to the origin
obj.SetTransform(o_wrt_w)
# compute the real desired tool_point
# use relative tool trans to compute a new trans in the new object transform
desired_tool_wrt_w= np.dot(get_trans(obj),gtrans)
desired_ee_world = compute_Tee_at_given_Ttool(desired_tool_wrt_w, manip.GetLocalToolTransform())
ee.SetTransform(desired_ee_world) # visualize
### Done! Now test IK
obj.Enable(False)
g_config = inverse_kinematics_helper(env, robot, desired_tool_wrt_w) # solve for ee pose
set_config(robot, g_config, robot.GetActiveManipulator().GetArmIndices())
height = 4
objB = box_body(env,width,length,height,\
name='objB',\
color=(1, 1, 1))
env.Add(objB)
set_point(objB,[1,0,0])
posB = objB.ComputeAABB().pos()
extB = objB.ComputeAABB().extents()
k=1
vtx = box_body(env,0.1,0.1,0.1,name='vtx+%d'%(k),color=(1,0,0))
env.Add(vtx)
set_point(vtx,posB+extB)
# vtx rel to objB?
vtx_wrt_B = np.linalg.solve(get_trans(objB),np.concatenate([get_point(vtx), np.array([1])]))
set_quat(objB,quat_from_z_rot(PI/2))
new_point= translate_point(get_trans(objB),vtx_wrt_B)
set_point(vtx,new_point)
import pdb;pdb.set_trace()
### Notations
# w = world
# o = obj
# ee = end-effector
def compute_tool_trans_wrt_obj_trans(tool_trans_wrt_world,object_trans):
return np.linalg.solve(object_trans, tool_trans_wrt_world)
def compute_Tee_at_given_Ttool( tool_trans_wrt_world,tool_trans_wrt_ee ):
# computes the endeffector transform at the given tool transform
return np.dot(tool_trans_wrt_world, np.linalg.inv(tool_trans_wrt_ee))
def sample_placement_using_gen(env,
obj,
robot,
p_samples,
obj_region,
robot_region,
GRAB_SLEEP_TIME=0.05):
# This script, with a given grasp of an object,
# - finding colfree obj placement within 100 tries. no robot at this point
# - checking colfree ik solution for robot; if not, trial is over
# - if above two passes, colfree path finding; if not, trial is over
status = "Failed"
path = None
original_trans = robot.GetTransform()
obj_orig_trans = obj.GetTransform()
tried = []
n_trials = 1 # try 5 different samples of placements with a given grasp
T_r_wrt_o = np.dot(np.linalg.inv(obj.GetTransform()), robot.GetTransform())
for _ in range(n_trials):
#print 'releasing obj'
sleep(GRAB_SLEEP_TIME)
robot.Release(obj)
robot.SetTransform(original_trans)
obj.SetTransform(obj_orig_trans)
#print 'released'
# get robot pose wrt obj
# sample obj pose
#print 'randmly placing obj'
#obj_xytheta = randomly_place_in_region(env,obj,obj_region) # randomly place obj
inCollision = True
np.random.shuffle(p_samples)
for idx, obj_xytheta in enumerate(p_samples):
if idx > 100: break
x = obj_xytheta[0]
y = obj_xytheta[1]
z = get_point(obj)[-1]
set_point(obj, [x, y, z])
th = obj_xytheta[2]
set_quat(obj, quat_from_z_rot(th))
new_T_robot = np.dot(obj.GetTransform(), T_r_wrt_o)
robot.SetTransform(new_T_robot)
inCollision = (check_collision_except(obj,robot,env))\
or (check_collision_except(robot,obj,env))
inRegion = (robot_region.contains(robot.ComputeAABB())) and\
(obj_region.contains(obj.ComputeAABB()))
if (not inCollision) and inRegion:
break
if inCollision or not (inRegion):
break # if you tried all p samples and ran out, get new pick
# compute the resulting robot transform
sleep(GRAB_SLEEP_TIME)
robot.Grab(obj)
robot.SetTransform(new_T_robot)
robot.SetActiveDOFs([],
DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis,
[0, 0, 1])
robot_xytheta = robot.GetActiveDOFValues()
robot.SetTransform(original_trans)
stime = time.time()
for node_lim in [1000, 5000, np.inf]:
path,tpath,status = get_motion_plan(robot,\
robot_xytheta,env,maxiter=10,n_node_lim=node_lim)
if path == 'collision':
# import pdb;pdb.set_trace()
pass
if status == "HasSolution":
robot.SetTransform(new_T_robot)
return obj_xytheta, robot_xytheta, path
else:
print('motion planning failed', tpath)
sleep(GRAB_SLEEP_TIME)
robot.Grab(obj)
robot.SetTransform(original_trans)
print("Returnining no solution")
return None, None, None
def rearrangement_problem(env,obj_poses=None):
# import random
# seed = 50
# random.seed(seed)
# np.random.seed(seed)
MIN_DELTA = .01 # .01 | .02
ENV_FILENAME = ENVIRONMENTS_DIR+'/single_table.xml'
env.Load(ENV_FILENAME)
robot = env.GetRobots()[0]
TABLE_NAME = 'table1'
NUM_OBJECTS = 8
WIDTH = .05 # .07 | .1
TARGET_WIDTH = 0.07
OBJ_HEIGHT = 0.2
objects = [box_body(env, WIDTH, WIDTH, .2, name='obj%s'%i, \
color=(0, (i+.5)/NUM_OBJECTS, 0)) for i in range(NUM_OBJECTS)]
target_obj = box_body(env, TARGET_WIDTH, TARGET_WIDTH, .2, name='target_obj', \
color=(1, 1.5, 0))
#TODO: Place obstacle that prevent you to reach from top
OBST_X = 0
OBST_WIDTH = 1.
OBST_COLOR = (1, 0, 0)
OBST_HEIGHT = 0.4
OBST_TRANSPARENCY = .25
place_body(env, box_body(env, .4, .05, OBST_HEIGHT, name='obst1', color=OBST_COLOR, transparency=OBST_TRANSPARENCY),
(.0, OBST_X-(OBST_WIDTH-.05)/2, 0), TABLE_NAME)
place_body(env, box_body(env, .4, .05, OBST_HEIGHT, name='obst2', color=OBST_COLOR, transparency=OBST_TRANSPARENCY),
(.0, OBST_X+(OBST_WIDTH-.05)/2, 0), TABLE_NAME)
place_body(env, box_body(env, .05, OBST_WIDTH, OBST_HEIGHT, name='obst3', color=OBST_COLOR, transparency=OBST_TRANSPARENCY),
(.225, OBST_X, 0), TABLE_NAME)
# roof
OBST_Z = OBST_HEIGHT
place_xyz_body(env, box_body(env, .4, OBST_WIDTH, .01, name='obst4', color=OBST_COLOR, transparency=OBST_TRANSPARENCY),
(0, OBST_X, OBST_Z,0), TABLE_NAME)
# I think this can be done once and for all
REST_TORSO = [.15]
robot.SetDOFValues(REST_TORSO, [robot.GetJointIndex('torso_lift_joint')])
l_model = databases.linkstatistics.LinkStatisticsModel(robot)
if not l_model.load(): l_model.autogenerate()
l_model.setRobotWeights()
min_delta = 0.01
l_model.setRobotResolutions(xyzdelta=min_delta) # xyzdelta is the minimum Cartesian distance of an object
extrema = aabb_extrema(aabb_union([aabb_from_body(body) for body in env.GetBodies()])).T
robot.SetAffineTranslationLimits(*extrema)
X_PERCENT = 0.0
# define intial region for movable objects
init_region = create_region(env, 'init_region', ((-0.8, 0.8), (-0.7, 0.6)), TABLE_NAME, color=np.array((1, 0, 1, .5)))
#init_region.draw(env)
# define target object region
target_obj_region = create_region(env, 'target_obj_region', ((-0.2, 0.6), (-0.5,0.5)), \
TABLE_NAME, color=np.array((0, 0, 0, 0.9)))
target_obj_region.draw(env)
# place object
if obj_poses is not None:
for obj in objects:
set_pose(obj,obj_poses[get_name(obj)])
set_quat(obj, quat_from_z_rot(0))
if env.GetKinBody(get_name(obj)) is None: env.Add(obj)
set_pose( target_obj, obj_poses[get_name(target_obj)] )
set_quat( target_obj, quat_from_z_rot(0) )
if env.GetKinBody(get_name(target_obj)) is None: env.Add(target_obj)
else:
for obj in objects:
randomly_place_region(env, obj, init_region)
set_quat(obj, quat_from_z_rot(0))
randomly_place_region(env, target_obj, target_obj_region)
set_quat(target_obj, quat_from_z_rot(0))
object_names = [get_name(body) for body in objects] # (tothefront,totheback), (to_the_right,to_the_left)
set_xy(robot, -.75, 0)
# LEFT_ARM_CONFIG = HOLDING_LEFT_ARM
robot.SetDOFValues(REST_LEFT_ARM, robot.GetManipulator('leftarm').GetArmIndices())
robot.SetDOFValues(mirror_arm_config(REST_LEFT_ARM), robot.GetManipulator('rightarm').GetArmIndices())
grasps = {}
for obj_name in object_names:
obj = env.GetKinBody(obj_name)
obj_grasps = get_grasps(env, robot, obj, GRASP_APPROACHES.SIDE, GRASP_TYPES.TOUCH) \
+ get_grasps(env, robot, obj, GRASP_APPROACHES.TOP, GRASP_TYPES.TOUCH)
grasps[get_name(obj)] = obj_grasps
target_obj = env.GetKinBody('target_obj')
target_obj_grasps = get_grasps(env, robot, target_obj, GRASP_APPROACHES.TOP, GRASP_TYPES.TOUCH)+\
get_grasps(env, robot, target_obj, GRASP_APPROACHES.SIDE, GRASP_TYPES.TOUCH)
grasps['target_obj'] = target_obj_grasps
initial_poses={}
for obj in objects:
initial_poses[get_name(obj)] = get_pose(obj)
initial_poses[get_name(target_obj)] = get_pose(target_obj)
return ManipulationProblem(None,
object_names=object_names, table_names='table1',
goal_regions=init_region,grasps=grasps,
initial_poses = initial_poses)
