def reposition_robot(self,hook_location,turn_angle,hook_angle,position_number=2):
h = self.workspace_dict[hook_angle]['height']
max_z_height = 1.3
min_z_height = 0.5
h_desired = self.z.get_position_meters()+hook_location[2,0]-h
print 'h_desired:',h_desired
print 'h:',h
h_achieve = ut.bound(h_desired,max_z_height,min_z_height)
h_err = h_desired-h_achieve
h = h+h_err
print 'h_achieve:',h_achieve
wkd = self.workspace_dict[hook_angle]['pts']
k = wkd.keys()
k_idx = np.argmin(np.abs(np.array(k)-h))
wrkspc_pts = wkd[k[k_idx]]
# good_location = np.matrix([0.45,-0.35,h]).T
good_location = wrkspc_pts.mean(1)
good_location = np.matrix(good_location.A1.tolist()+[h]).T
if position_number == 1:
good_location[0,0] += 0.0
good_location[1,0] -= 0.1
elif position_number == 2:
good_location[0,0] += 0.0
good_location[1,0] -= 0.0
elif position_number == 3:
good_location[0,0] += 0.0
good_location[1,0] += 0.1
else:
good_location[0,0] -= 0.1
good_location[1,0] -= 0.0
good_location = mcf.mecanumTglobal(mcf.globalTtorso(good_location))
hook_location = mcf.mecanumTglobal(mcf.globalTtorso(hook_location))
v = tr.Rz(-turn_angle)*good_location
move_vector = hook_location - v
pose1 = self.segway_pose.get_pose()
self.segway_command_node.go_xya_pos_local(move_vector[0,0],move_vector[1,0],
turn_angle,blocking=False)
self.z.torque_move_position(h_achieve)
print 'before waiting for segway to stop moving.'
self.segway_command_node.wait_for_tolerance_pos(0.03,0.03,math.radians(4))
print 'after segway has stopped moving.'
pose2 = self.segway_pose.get_pose()
hl_new = vop.transform_pts_local(hook_location[0:2,:],pose1,pose2)
h_err = h_desired-self.z.get_position_meters()
g = np.matrix(hl_new.A1.tolist()+[good_location[2,0]+h_err]).T
g = mcf.torsoTglobal(mcf.globalTmecanum(g))
angle_turned = pose2[2]-pose1[2]
angle_error = tr.angle_within_mod180(turn_angle-angle_turned)
self.segway_pose.set_origin() # re-origining for the manipulation behaviors
return g, angle_error
def tlRts(vecs_ts, a):
vecs_ms = mcf.mecanumTglobal(mcf.globalTtorso(vecs_ts, True), True)
vecs_ml = tr.Rz(a) * vecs_ms
vecs_tl = mcf.torsoTglobal(mcf.globalTmecanum(vecs_ml, True), True)
return vecs_tl
hook_location = None
#z_list = [1.0,0.5]
z_list = [opt.z]
i = 0
while hook_location == None:
if i == len(z_list):
print 'Did not get a click. Exiting...'
sys.exit()
z = z_list[i]
cmg.z.torque_move_position(z)
hook_location = lpi.select_location(cmg.cam,cmg.thok,math.radians(sa))
i += 1
hl_thok0 = mcf.thok0Tglobal(hook_location)
hl_torso = mcf.torsoTglobal(hook_location)
t_begin = time.time()
angle = 0.
pull_loc,residual_angle = cmg.reposition_robot(hl_torso,angle,math.radians(ha),
position_number=pnum)
print 'pull_loc:',pull_loc.A1.tolist()
starting_location = copy.copy(hl_torso)
starting_angle = -angle
pose_dict = {}
pose_dict['loc'] = starting_location
pose_dict['angle'] = angle
pose_dict['residual_angle'] = residual_angle
pose_dict['position_number'] = pnum
def tsTtl(pts_tl,x,y,a):
pts_ml = mcf.mecanumTglobal(mcf.globalTtorso(pts_tl))
v_org_ms = np.matrix([x,y,0.]).T
pts_ms = tr.Rz(-a) * pts_ml + v_org_ms
pts_ts = mcf.torsoTglobal(mcf.globalTmecanum(pts_ms))
return pts_ts
def tsRtl(vecs_tl, a):
vecs_ml = mcf.mecanumTglobal(mcf.globalTtorso(vecs_tl, True), True)
vecs_ms = tr.Rz(-a) * vecs_ml
vecs_ts = mcf.torsoTglobal(mcf.globalTmecanum(vecs_ms, True), True)
return vecs_ts
def tlTts(pts_ts,x,y,a):
pts_ms = mcf.mecanumTglobal(mcf.globalTtorso(pts_ts))
v_org_ms = np.matrix([x,y,0.]).T
pts_ml = tr.Rz(a)*(pts_ms-v_org_ms)
pts_tl = mcf.torsoTglobal(mcf.globalTmecanum(pts_ml))
return pts_tl
def tlRts(vecs_ts, a):
vecs_ms = mcf.mecanumTglobal(mcf.globalTtorso(vecs_ts, True), True)
vecs_ml = tr.Rz(a) * vecs_ms
vecs_tl = mcf.torsoTglobal(mcf.globalTmecanum(vecs_ml, True), True)
return vecs_tl
def tsRtl(vecs_tl, a):
vecs_ml = mcf.mecanumTglobal(mcf.globalTtorso(vecs_tl, True), True)
vecs_ms = tr.Rz(-a) * vecs_ml
vecs_ts = mcf.torsoTglobal(mcf.globalTmecanum(vecs_ms, True), True)
return vecs_ts
def tsTtl(pts_tl, x, y, a):
pts_ml = mcf.mecanumTglobal(mcf.globalTtorso(pts_tl))
v_org_ms = np.matrix([x, y, 0.]).T
pts_ms = tr.Rz(-a) * pts_ml + v_org_ms
pts_ts = mcf.torsoTglobal(mcf.globalTmecanum(pts_ms))
return pts_ts
def tlTts(pts_ts, x, y, a):
pts_ms = mcf.mecanumTglobal(mcf.globalTtorso(pts_ts))
v_org_ms = np.matrix([x, y, 0.]).T
pts_ml = tr.Rz(a) * (pts_ms - v_org_ms)
pts_tl = mcf.torsoTglobal(mcf.globalTmecanum(pts_ml))
return pts_tl
