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
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 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