def from_file_multisweep( cls, nusc, sample_rec: str, chan: str, ref_chan: str, nsweeps: int = 26, min_distance: float = 1.0) -> Tuple[PointCloud, np.ndarray]: """ Return a point cloud that aggregates multiple sweeps. As every sweep is in a different coordinate frame, we need to map the coordinates to a single reference frame. As every sweep has a different timestamp, we need to account for that in the transformations and timestamps. :param nusc: A NuScenes instance. :param sample_rec: The current sample. :param chan: The radar channel from which we track back n sweeps to aggregate the point cloud. :param ref_chan: The reference channel of the current sample_rec that the point clouds are mapped to. :param nsweeps: Number of sweeps to aggregated. :param min_distance: Distance below which points are discarded. :return: (all_pc, all_times). The aggregated point cloud and timestamps. """ # Init points = np.zeros((cls.nbr_dims(), 0)) all_pc = cls(points) all_times = np.zeros((1, 0)) # Get reference pose and timestamp ref_sd_token = sample_rec['data'][ref_chan] ref_sd_rec = nusc.get('sample_data', ref_sd_token) ref_pose_rec = nusc.get('ego_pose', ref_sd_rec['ego_pose_token']) ref_cs_rec = nusc.get('calibrated_sensor', ref_sd_rec['calibrated_sensor_token']) ref_time = 1e-6 * ref_sd_rec['timestamp'] # Homogeneous transform from ego car frame to reference frame ref_from_car = transform_matrix(ref_cs_rec['translation'], Quaternion(ref_cs_rec['rotation']), inverse=True) # Homogeneous transformation matrix from global to _current_ ego car frame car_from_global = transform_matrix(ref_pose_rec['translation'], Quaternion( ref_pose_rec['rotation']), inverse=True) # Aggregate current and previous sweeps. sample_data_token = sample_rec['data'][chan] current_sd_rec = nusc.get('sample_data', sample_data_token) for _ in range(nsweeps): # Load up the pointcloud. current_pc = cls.from_file( osp.join(nusc.dataroot, current_sd_rec['filename'])) # Get past pose. current_pose_rec = nusc.get('ego_pose', current_sd_rec['ego_pose_token']) global_from_car = transform_matrix( current_pose_rec['translation'], Quaternion(current_pose_rec['rotation']), inverse=False) # Homogeneous transformation matrix from sensor coordinate frame to ego car frame. current_cs_rec = nusc.get( 'calibrated_sensor', current_sd_rec['calibrated_sensor_token']) car_from_current = transform_matrix( current_cs_rec['translation'], Quaternion(current_cs_rec['rotation']), inverse=False) # Fuse four transformation matrices into one and perform transform. trans_matrix = reduce(np.dot, [ ref_from_car, car_from_global, global_from_car, car_from_current ]) current_pc.transform(trans_matrix) # Remove close points and add timevector. current_pc.remove_close(min_distance) time_lag = ref_time - 1e-6 * current_sd_rec[ 'timestamp'] # positive difference times = time_lag * np.ones((1, current_pc.nbr_points())) all_times = np.hstack((all_times, times)) # Merge with key pc. all_pc.points = np.hstack((all_pc.points, current_pc.points)) # Abort if there are no previous sweeps. if current_sd_rec['prev'] == '': break else: current_sd_rec = nusc.get('sample_data', current_sd_rec['prev']) return all_pc, all_times
def test_init_default(self): q = Quaternion() self.assertIsInstance(q, Quaternion) self.assertEqual(q, Quaternion(1., 0., 0., 0.))
def test_output_of_elements(self): r = randomElements() q = Quaternion(*r) self.assertEqual(tuple(q.elements), r)
def test_float(self): a, b, c, d = randomElements() q = Quaternion(a, b, c, d) self.assertEqual(float(q), a)
def test_equality(self): r = randomElements() self.assertEqual(Quaternion(*r), Quaternion(*r)) q = Quaternion(*r) self.assertEqual(q, q) # Equality should work with other types, if they can be interpreted as quaternions self.assertEqual(q, r) self.assertEqual(Quaternion(1., 0., 0., 0.), 1.0) self.assertEqual(Quaternion(1., 0., 0., 0.), "1.0") self.assertNotEqual(q, q + Quaternion(0.0, 0.002, 0.0, 0.0)) # Equality should also cover small rounding and floating point errors self.assertEqual(Quaternion(1., 0., 0., 0.), Quaternion(1.0 - 1e-14, 0., 0., 0.)) self.assertNotEqual(Quaternion(1., 0., 0., 0.), Quaternion(1.0 - 1e-12, 0., 0., 0.)) self.assertNotEqual(Quaternion(160., 0., 0., 0.), Quaternion(160.0 - 1e-10, 0., 0., 0.)) self.assertNotEqual(Quaternion(1600., 0., 0., 0.), Quaternion(1600.0 - 1e-9, 0., 0., 0.)) with self.assertRaises(TypeError): q == None with self.assertRaises(ValueError): q == 's'
def test_init_from_explicit_rotation_params(self): vx = random() vy = random() vz = random() theta = random() * 2.0 * pi v1 = (vx, vy, vz) # tuple format v2 = [vx, vy, vz] # list format v3 = np.array(v2) # array format q1 = Quaternion(axis=v1, angle=theta) q2 = Quaternion(axis=v2, radians=theta) q3 = Quaternion(axis=v3, degrees=theta / pi * 180) # normalise v to a unit vector v3 = v3 / np.linalg.norm(v3) q4 = Quaternion(angle=theta, axis=v3) # Construct the true quaternion t = theta / 2.0 a = cos(t) b = v3[0] * sin(t) c = v3[1] * sin(t) d = v3[2] * sin(t) truth = Quaternion(a, b, c, d) self.assertEqual(q1, truth) self.assertEqual(q2, truth) self.assertEqual(q3, truth) self.assertEqual(q4, truth) self.assertEqual(Quaternion(axis=v3, angle=0), Quaternion()) self.assertEqual(Quaternion(axis=v3, radians=0), Quaternion()) self.assertEqual(Quaternion(axis=v3, degrees=0), Quaternion()) self.assertEqual(Quaternion(axis=v3), Quaternion()) # Result should be a versor (Unit Quaternion) self.assertAlmostEqual(q1.norm, 1.0, ALMOST_EQUAL_TOLERANCE) self.assertAlmostEqual(q2.norm, 1.0, ALMOST_EQUAL_TOLERANCE) self.assertAlmostEqual(q3.norm, 1.0, ALMOST_EQUAL_TOLERANCE) self.assertAlmostEqual(q4.norm, 1.0, ALMOST_EQUAL_TOLERANCE) with self.assertRaises(ValueError): q = Quaternion(angle=theta) with self.assertRaises(ValueError): q = Quaternion(axis=[b, c], angle=theta) with self.assertRaises(ValueError): q = Quaternion(axis=(b, c, d, d), angle=theta) with self.assertRaises(ZeroDivisionError): q = Quaternion(axis=[0., 0., 0.], angle=theta)
def test_repr(self): a, b, c, d = np.array(randomElements()) # Numpy seems to increase precision of floats (C magic?) q = Quaternion(a, b, c, d) string = "Quaternion(" + repr(a) + ", " + repr(b) + ", " + repr(c) + ", " + repr(d) + ")" self.assertEqual(string, repr(q))
def QP_abbirb6640(q, v, check_flag=False, collision_checker=None): # Init the joystick # Initialize Robot Parameters ex, ey, ez, n, P, q_ver, H, ttype, dq_bounds = robotParams() # joint limits lower_limit = np.transpose( np.array([ -360 * np.pi / 180, -360 * np.pi / 180, -2 * np.pi, -300 * np.pi / 180, -180 * np.pi / 180, -2 * np.pi ])) upper_limit = np.transpose( np.array([ 360 * np.pi / 180, 360 * np.pi / 180, 360 * np.pi / 180, 300 * np.pi / 180, 180 * np.pi / 180, 2 * np.pi ])) # Initialize Control Parameters # initial joint angles pos_v = np.zeros((3, 1)) ang_v = np.array([1, 0, 0, 0]) dq = np.zeros((int(n), 1)) # inequality constraints # we use h to calculate the sigma, so h should also be a vector of (13*1) # h consists of 6 upper bound, 6 lower bound and 1 distance constraints h = np.zeros((17, 1)) sigma = np.zeros((17, 1)) dhdq = np.vstack((np.hstack((np.eye(6), np.zeros((6, 1)), np.zeros( (6, 1)))), np.hstack((-np.eye(6), np.zeros((6, 1)), np.zeros( (6, 1)))), np.zeros((5, 8)))) # velocities w_t = np.zeros((3, 1)) v_t = np.zeros((3, 1)) # keyboard controls # define position and angle step inc_pos_v = 0.01 # m/s inc_ang_v = 0.5 * np.pi / 180 # rad/s # optimization params er = 0.05 ep = 0.05 epsilon = 0 # legacy param for newton iters # parameters for inequality constraints c = 0.5 eta = 0.1 epsilon_in = 0.15 E = 0.005 pp, RR = fwdkin_alljoints(q, ttype, H, P, n) orien_tmp = Quaternion(matrix=RR[:, :, -1]) orien_tmp = np.array( [orien_tmp[0], orien_tmp[1], orien_tmp[2], orien_tmp[3]]).reshape(1, 4) # create a handle of these parameters for interactive modifications obj = ControlParams(ex, ey, ez, n, P, H, ttype, dq_bounds, q, dq, pp[:, -1], orien_tmp, pos_v, ang_v.reshape(1, 4), w_t, v_t, epsilon, inc_pos_v, inc_ang_v, 0, er, ep, 0) J_eef = getJacobian(obj.params['controls']['q'], obj.params['defi']['ttype'], obj.params['defi']['H'], obj.params['defi']['P'], obj.params['defi']['n']) # desired rotational velocity vr = v[0:3] # desired linear velocity vp = v[3:6] Q = getqp_H_new(J_eef, vr.reshape(3, 1), vp.reshape(3, 1), obj.params['opt']['er'], obj.params['opt']['ep']) # make sure Q is symmetric Q = 0.5 * (Q + Q.T) f = getqp_f_new(obj.params['opt']['er'], obj.params['opt']['ep']) f = f.reshape((8, )) # bounds for qp if obj.params['opt']['upper_dq_bounds']: bound = obj.params['defi']['dq_bounds'][1, :] else: bound = obj.params['defi']['dq_bounds'][0, :] LB = np.vstack((-0.1 * bound.reshape(6, 1), 0, 0)) UB = np.vstack((0.1 * bound.reshape(6, 1), 1, 1)) # inequality constrains A and b h[0:6] = obj.params['controls']['q'] - lower_limit.reshape(6, 1) h[6:12] = upper_limit.reshape(6, 1) - obj.params['controls']['q'] sigma[0:12] = inequality_bound(h[0:12], c, eta, epsilon_in, E) # collision constraint if check_flag: # (Closest_Pt, Closest_Pt_env) = collision_checker.collision_report() closest_Pts = collision_checker.collision_report() i = 12 for key, value in closest_Pts.items(): Closest_Pt, Closest_Pt_env = value dist = numpy.linalg.norm(Closest_Pt_env - Closest_Pt) ## TODO: check several points instead of one point dx = Closest_Pt_env[0] - Closest_Pt[0] dy = Closest_Pt_env[1] - Closest_Pt[1] dz = Closest_Pt_env[2] - Closest_Pt[2] # dz = 0 """ """ dist = np.sqrt(dx**2 + dy**2 + dz**2) # assert dist == dist1 # derivative of dist w.r.t time der = np.array([dx / dist, dy / dist, dz / dist]) h[i] = dist dhdq[i, 0:6] = np.dot(-der[None, :], J_eef[3:6, :]) # TODO: here I changed the equation of the sigma eta = 0.03 sigma[i] = inequality_bound(h[i], c, eta, epsilon_in, E) # sigma[i] = h[i] i += 1 # break A = np.vstack((dhdq, np.eye(8), -np.eye(8))) b = np.vstack((sigma, LB, -UB)) # solve the quadprog problem try: dq_sln = quadprog.solve_qp(Q, -f, A.T, b.reshape((33, )))[0] except: return [0, 0, 0, 0, 0, 0] if len(dq_sln) < obj.params['defi']['n']: obj.params['controls']['dq'] = np.zeros((6, 1)) V_scaled = 0 print 'No Solution' dq_sln = np.zeros((int(n), 1)) else: obj.params['controls']['dq'] = dq_sln[0:int(obj.params['defi']['n'])] obj.params['controls']['dq'] = obj.params['controls']['dq'].reshape( (6, 1)) #print dq_sln # V_scaled = dq_sln[-1]*V_desired # vr_scaled = dq_sln[-2]*vr.reshape(3,1) #print np.dot(np.linalg.pinv(J_eef),v) # get the linear velocity and angular velocity V_linear = np.dot(J_eef[3:6, :], obj.params['controls']['dq']) V_rot = np.dot(J_eef[0:3, :], obj.params['controls']['dq']) return dq_sln[0:6]
def main(): # OpenRAVE_obj = OpenRAVEObject() # Initialize Robot Parameters ex, ey, ez, n, P, q_ver, H, ttype, dq_bounds = robotParams() # Initialize Control Parameters # initial joint angles """ """ # Q_all = np.array([[-1.5832209587097168, 0.35797879099845886, -0.018985196948051453, 3.492117684800178e-05, 1.2321044206619263, -0.012819867581129074], # [-1.0001745223999023, 0.3400745689868927, 0.27556654810905457, -0.01793256774544716, 0.8835334777832031, -0.06672105938196182], # [-0.4340008497238159, 0.33434557914733887, 0.2755616009235382, -0.01793256774544716, 0.8835381269454956, -0.06672105938196182], # [-0.08396485447883606, 0.33433452248573303, 0.2755637466907501, -0.017921535298228264, 0.8835418224334717, -0.06673289835453033], # [0.027317576110363007, 0.7219697833061218, -0.3032461106777191, -0.025421472266316414, 1.0768276453018188, 0.044921014457941055], # [0.5158949494361877, 1.2091666460037231, -0.9566897749900818, -0.04086354747414589, 1.269188404083252, 0.5332122445106506]]) # Q_all = np.array([[-1.4735171794891357, 0.43576669692993164, 0.025293484330177307, 0.012627056799829006, 1.0394766330718994, 0.0606585256755352], # [-1.040715515613556, 0.43576669692993164, 0.025293484330177307, 0.012627056799829006, 1.0394766330718994, 0.0606585256755352], # [-0.6079138517379761, 0.4357639253139496, 0.025260837748646736, 0.030262663960456848, 1.039481282234192, 0.060406409204006195], # [-0.607966959476471, 0.5187827348709106, 0.12658417224884033, 0.03462858498096466, 0.8553994297981262, -0.6607409119606018], # [-0.4619627594947815, 1.0956398248672485, -0.7398647665977478, 0.01724303886294365, 1.1420093774795532, -0.49965518712997437]]) Q_all = np.array([[ -1.5832209587097168, 0.35797879099845886, -0.018985196948051453, 3.492117684800178e-05, 1.2321044206619263, -0.012819867581129074 ], [ -1.040715515613556, 0.35797879099845886, -0.018985196948051453, 3.492117684800178e-05, 1.2321044206619263, -0.012819867581129074 ], [ -0.6079138517379761, 0.35797879099845886, -0.018985196948051453, 3.492117684800178e-05, 1.2321044206619263, -0.012819867581129074 ], [ -0.607966959476471, 0.5187827348709106, 0.12658417224884033, 0.03462858498096466, 0.8553994297981262, -0.6607409119606018 ], [ -0.4619627594947815, 1.0956398248672485, -0.7398647665977478, 0.01724303886294365, 1.1420093774795532, -0.49965518712997437 ]]) test_q = np.array([4.8046852, 2.92482, 1.002, 4.2031852, 1.4458, 1.3233]) test_q = np.array([3.464029, 2.65480, 0.514936, 3.10589, 1.3006, 2.48233]) test_q = np.array([ 0.0019249955138216472, 3.1432288997635376, 3.139667720753884, 3.1451626210310444, 3.1404027532147456, 0.004759989010934167 ]) test_q = np.array([0., pi, pi, pi, pi, 0]) # calibrate 1: # real x: -0.00213531428017 y: 0.0643087550998 z: 1.18114089966 # predict x: -1.71608523e-16 y: 6.43000000e-02 z: 1.18114496e+00 # test_q = np.array([0., pi, pi, 0, 0, 0]) # calibrate 2: # real: x: 0.498562157154 y: 0.0647798776627 z: 0.679222285748 # our predict: x: 0.49564496 y: 0.0643 z: 0.6855 # test_q = np.array([0., pi, 1.5*pi, 0, 0, 0]) # calibrate 3: # real x: 0.398912936449 y: -0.305943399668 z: 0.677314043045 # our predict x: 0.39594088 y: -0.30500695 z: 0.6855 # test_q = np.array([pi/4., pi, 1.5*pi, 0, 0, 0]) # calibrate 4: # real: x: 0.349278271198 y: -0.256594508886 z: 0.431439548731 # our predict: x: 0.34898628 y: -0.25805235 z: 0.43767752 # test_q = np.array([pi/4., pi, pi*5/3, 0, 0, 0]) # 45, 180, 300, 0, 0, 0 # calibrate 5: # real: x: 0.382904559374 y: -0.165705829859 z: 0.657457232475 # predict: x: 0.38003075 y: -0.16491893 z:0.66580688 test_q = np.array([pi / 4., pi, pi * 5 / 3, 0, pi / 2, 0]) # 45, 180, 300, 0, 90, 0 # calibrate 6: # real x: 0.50574274 y: -0.27025385 z: 0.85814328 # predict: match test_q = np.array([ 0.5149363115205982, 2.7840728646265798, 4.284077575957208, -0.5259787760854754, 0.860368012295942, 0.1713596023130354 ]) # current bullet environment joint angle test_q = np.array([ 2.08477017189754, 2.94851560508792, 0.9403603166942158, 4.0745507140341335, 0.7758782280277694, 8.224071010650652 - 2 * pi ]) test_q = np.array([ 2.9654557064658054, 2.9599693283041333, 0.5794236254734035, 3.9531707557671565, 1.8754357233632242, 1.953975539702969 ]) test_q = test_q.reshape(6, 1) # q = Q_all[0,:].reshape(6, 1) q = test_q Checker = CollisionCheck() Checker.simulation2() # without checker: [-0.17453293 -0.005 0.07432923 -0.03735876 -0.18331453 0.03045566] # with collision checker: [-0.0049198 -0.1234 -0.00917626 0.02173149 -0.0389669 -0.09331467] result = QP_abbirb6640(q, np.array([0, 0, 0, 0, 0.3, 0]), True, Checker) print result R, pos = fwdkin(test_q, ttype, H, P, n) print(pos) R, pos = fwdkin(test_q + np.array(result), ttype, H, P, n) print(pos) # sys.exit(1) orien = Quaternion(matrix=R) orien = np.array([orien[0], orien[1], orien[2], orien[3]]).reshape(1, 4) pos_v = np.zeros((3, 1)) ang_v = np.array([1, 0, 0, 0]) dq = np.zeros((int(n), 1)) # joint limits lower_limit = np.transpose( np.array([ -170 * np.pi / 180, -65 * np.pi / 180, -np.pi, -300 * np.pi / 180, -120 * np.pi / 180, -2 * np.pi ])) upper_limit = np.transpose( np.array([ 170 * np.pi / 180, 85 * np.pi / 180, 70 * np.pi / 180, 300 * np.pi / 180, 120 * np.pi / 180, 2 * np.pi ])) # inequality constraints h = np.zeros((15, 1)) sigma = np.zeros((13, 1)) dhdq = np.vstack((np.hstack((np.eye(6), np.zeros((6, 1)), np.zeros( (6, 1)))), np.hstack((-np.eye(6), np.zeros((6, 1)), np.zeros( (6, 1)))), np.zeros((1, 8)))) # velocities w_t = np.zeros((3, 1)) v_t = np.zeros((3, 1)) # keyboard controls # define position and angle step inc_pos_v = 0.01 # m/s inc_ang_v = 0.5 * np.pi / 180 # rad/s # optimization params er = 0.05 ep = 0.05 epsilon = 0 # legacy param for newton iters # parameters for inequality constraints c = 0.5 eta = 0.1 epsilon_in = 0.15 E = 0.005 Ke = 1 # create a handle of these parameters for interactive modifications obj = ControlParams(ex, ey, ez, n, P, H, ttype, dq_bounds, q, dq, pos, orien, pos_v, ang_v.reshape(1, 4), w_t, v_t, epsilon, inc_pos_v, inc_ang_v, 0, er, ep, 0) dt = 0 counter = 0 # dL = 1.0/200.0 dL = 1.0 / 5.0 Lambda = np.linspace(0, 1, 1.0 / dL + 1) #np.linspace(0,1,1001) pos_b = pos R_b = R q_b = q eu_b = np.array(euler_from_matrix(R_b)) Joint_all = [] Checker = CollisionCheck() Checker.simulation2() # Q_all: is the path for the arm # for iq in range(Q_all.shape[0]-1): # here we just define two point, start point and end point: my_R, start_pos = fwdkin(test_q, ttype, H, P, n) cur_pos = start_pos end_pos = start_pos + np.array([0, 0.2, -0.]).reshape( (3, 1)) # move 10cm in the y direction # for iq in range(Q_all.shape[0]-1): index = 0 if 1: # cur_pos != end_pos: orien = Quaternion(matrix=my_R) iq = Checker.calculateInverseKinematics(end_pos, orien).reshape(6, 1) iq_copy = iq # Checker.update_joint(iq) q_a = q_b R_a = R_b pos_a = pos_b eu_a = eu_b # q_a = obj.params['controls']['q'] # R_a, pos_a = fwdkin(q_a,ttype,H,P,n) # eu_a = np.array(euler_from_matrix(R_a)) q_b = iq # Q_all[iq+1,:].reshape(6, 1) ## TODO: can not get correct pos_b from fwdkin(q_b). The q_b is from bullet, this function is our own function R_b, pos_b = fwdkin(iq, ttype, H, P, n) eu_b = np.array(euler_from_matrix(R_b)) #stop, Closest_Pt, Closest_Pt_env = OpenRAVE_obj.CollisionReport(obj.params['controls']['q'][0],obj.params['controls']['q'][1],obj.params['controls']['q'][2],obj.params['controls']['q'][3],obj.params['controls']['q'][4],obj.params['controls']['q'][5]) #raw_input('pause') for ld in Lambda: eu = eu_a * (1 - ld) + eu_b * ld R_des = euler_matrix(eu[0], eu[1], eu[2]) R_des = R_des[0:3, 0:3] #print R_des[0:3,0:3],x_des, eu x_des = pos_a * (1 - ld) + pos_b * ld x_des = np.array([x_des[0][0], x_des[1][0], x_des[2][0]]) if x_des[1] > 0.5: print obj.params['controls']['q'] = obj.params['controls'][ 'q'] + obj.params['controls']['dq'] #*dt*0.1 Joint_all.append(np.transpose(obj.params['controls']['q'])[0]) #print np.transpose(obj.params['controls']['q'])[0] pp, RR = fwdkin_alljoints(obj.params['controls']['q'], ttype, obj.params['defi']['H'], obj.params['defi']['P'], obj.params['defi']['n']) # parameters for qp obj.params['controls']['pos'] = pp[:, -1] orien_tmp = Quaternion(matrix=RR[:, :, -1]) obj.params['controls']['orien'] = np.array( [orien_tmp[0], orien_tmp[1], orien_tmp[2], orien_tmp[3]]).reshape(1, 4) (Closest_Pt, Closest_Pt_env) = Checker.collision_report() # stop, Closest_Pt, Closest_Pt_env = OpenRAVE_obj.CollisionReport(obj.params['controls']['q'][0],obj.params['controls']['q'][1],obj.params['controls']['q'][2],obj.params['controls']['q'][3],obj.params['controls']['q'][4],obj.params['controls']['q'][5]) # check self-collision # if (stop): # print 'robot is about to self-collide.' # #obj.params['controls']['pos_v'] = np.array([0,0,0]).reshape(3, 1) # #obj.params['controls']['ang_v'] = np.array([1,0,0,0]).reshape(1, 4) J2C_Joint = Joint2Collision(Closest_Pt, pp) J_eef = getJacobian(obj.params['controls']['q'], obj.params['defi']['ttype'], obj.params['defi']['H'], obj.params['defi']['P'], obj.params['defi']['n']) v_tmp = Closest_Pt - obj.params['controls']['pos'] v_tmp2 = (pp[:, -1] - pp[:, -3]) p_norm2 = norm(v_tmp2) v_tmp2 = v_tmp2 / p_norm2 # determine if the closest point is on the panel #print norm(v_tmp),np.arccos(np.inner(v_tmp, v_tmp2)/norm(v_tmp))*180/np.pi if (norm(v_tmp) < 2.5 and np.arccos(np.inner(v_tmp, v_tmp2) / norm(v_tmp)) * 180 / np.pi < 70): # print '---the closest point is on the panel---' J2C_Joint = 6 J = getJacobian3(obj.params['controls']['q'], obj.params['defi']['ttype'], obj.params['defi']['H'], obj.params['defi']['P'], obj.params['defi']['n'], Closest_Pt, J2C_Joint) #J,p_0_tmp = getJacobian2(obj.params['controls']['q'], obj.params['defi']['ttype'], obj.params['defi']['H'], obj.params['defi']['P'], obj.params['defi']['n'],Closest_Pt,J2C_Joint) #if (J2C_Joint < 4): else: J, p_0_tmp = getJacobian2(obj.params['controls']['q'], obj.params['defi']['ttype'], obj.params['defi']['H'], obj.params['defi']['P'], obj.params['defi']['n'], Closest_Pt, J2C_Joint) #else: # J = getJacobian3(obj.params['controls']['q'], obj.params['defi']['ttype'], obj.params['defi']['H'], obj.params['defi']['P'], obj.params['defi']['n'], Closest_Pt,J2C_Joint) # update joint velocities #axang = quat2axang(obj.params['controls']['ang_v']) # desired rotational velocity w_skew = logm(np.dot(RR[:, :, -1], R_des.T)) w = np.array([w_skew[2, 1], w_skew[0, 2], w_skew[1, 0]]) vr = -Ke * w obj.params['controls']['ang_v'] = vr # desired linear velocity V_desired = np.reshape(Ke * (x_des - pp[:, -1]), [3, 1]) obj.params['controls']['pos_v'] = V_desired #print x_des #print pp[:,-1] #print (x_des-np.reshape(pp[:,-1],[3,1])) Q = getqp_H(obj.params['controls']['dq'], J_eef, vr.reshape(3, 1), obj.params['controls']['pos_v'], obj.params['opt']['er'], obj.params['opt']['ep']) # make sure Q is symmetric Q = 0.5 * (Q + Q.T) f = getqp_f(obj.params['controls']['dq'], obj.params['opt']['er'], obj.params['opt']['ep']) f = f.reshape((8, )) # bounds for qp if obj.params['opt']['upper_dq_bounds']: bound = obj.params['defi']['dq_bounds'][1, :] else: bound = obj.params['defi']['dq_bounds'][0, :] LB = np.vstack((-0.1 * bound.reshape(6, 1), 0, 0)) UB = np.vstack((0.1 * bound.reshape(6, 1), 1, 1)) # LB = matrix(LB, tc = 'd') # UB = matrix(UB, tc = 'd') # inequality constrains A and b h[0:6] = obj.params['controls']['q'] - lower_limit.reshape(6, 1) h[6:12] = upper_limit.reshape(6, 1) - obj.params['controls']['q'] dx = Closest_Pt_env[0] - Closest_Pt[0] dy = Closest_Pt_env[1] - Closest_Pt[1] dz = Closest_Pt_env[2] - Closest_Pt[2] """ """ dist = np.sqrt(dx**2 + dy**2 + dz**2) # derivative of dist w.r.t time der = np.array([dx / dist, dy / dist, dz / dist]) """ """ h[12] = dist - 0.1 """ """ """ """ #dhdq[12, 0:6] = np.dot(-der.reshape(1, 3), J_eef2[3:6,:]) dhdq[12, 0:6] = np.dot(-der[None, :], J[3:6, :]) sigma[0:12] = inequality_bound(h[0:12], c, eta, epsilon_in, E) sigma[12] = inequality_bound(h[12], c, eta, epsilon_in, E) A = dhdq b = sigma A = np.vstack((A, np.eye(8), -np.eye(8))) b = np.vstack((b, LB, -UB)) b = b.reshape((29, )) # solve the quadprog problem #if not is_pos_def(Q): # print np.linalg.eigvals(Q) # dq_sln = np.zeros((6,1)) # raw_input('pause') # #else: dq_sln = quadprog.solve_qp(Q, -f, A.T, b)[0] if len(dq_sln) < obj.params['defi']['n']: obj.params['controls']['dq'] = np.zeros((6, 1)) V_scaled = 0 print 'No Solution' else: obj.params['controls']['dq'] = dq_sln[ 0:int(obj.params['defi']['n'])] obj.params['controls']['dq'] = obj.params['controls'][ 'dq'].reshape((6, 1)) V_scaled = dq_sln[-1] * V_desired vr_scaled = dq_sln[-2] * vr.reshape(3, 1) V_linear = np.dot(J_eef[3:6, :], obj.params['controls']['dq']) V_rot = np.dot(J_eef[0:3, :], obj.params['controls']['dq']) iq = Checker.calculateInverseKinematics(x_des, orien).reshape(6, 1) # Checker.update_joint(obj.params['controls']['q']) # print obj.params["controls"]['dq'] Checker.update_joint(iq) # print x_des # bullet approach to calculate the jacobian: (jac_t, jac_r) = Checker.calculateJacobian(iq_copy) print Checker.multiplyJacobian2(obj.params["controls"]['dq']) dq = np.concatenate((iq - iq_copy, np.array([[0], [0], [0]]))) result = dq.reshape(9) * jac_t # print '------Scaled desired linear velocity------' # # print V_scaled # # print '------Real linear velocity by solving quadratic programming------' # # print V_linear # print '------Position--------' # print x_des # print end_pos if index % 100 == 0: print index += 1 time.sleep(0.01) # print '------Scaled desired angular velocity------' # print vr_scaled # # print '------Real angular velocity by solving quadratic programming------' # print V_rot raw_input("pause") np.savetxt('Joint_all.out', Joint_all, delimiter=',')
def sampled_kvectors_spherical_coordinates(self, NA_in, NA_out, NumSample, kd): #sample multiple planewaves at different angle to do simulation as a focused beam # return a list of planewave direction vectors Kd # and the corresponding scale factor if using uniform sampling # NA: numberical aperture of the lens which planewaves are focused from # NumSample: number of samples(planewaves) # kd: center planewave of the focused beam #allocatgge space for the field and initialize it to zero start3 = time.time() CenterKd = self.k #defualt planewave coming in perpendicular to the surface kd = kd / np.linalg.norm(kd) #normalize the new planewave r = np.sqrt( CenterKd[0]**2 + CenterKd[1]**2 + CenterKd[2]**2 ) #radiance of the hemisphere where the k vectors are sampled from if ( kd[0] == CenterKd[0] and kd[1] == CenterKd[1] and kd[2] == CenterKd[2] ): #if new planewave is at the same direction as the default plane wave rotateAxis = CenterKd #set rotation axis as defualt k vector RoAngle = 0 #set rotation axis as 0 degrees else: #if new plane wave is at different direction as the defualt planewave, rotation is needed rotateAxis = np.cross( CenterKd, kd ) #find a axis which is perpendicular to both vectors to be rotation axis RoAngle = math.asin(kd[2] / r) #calculate the rotation angle beamRotate = Quaternion( axis=rotateAxis, angle=RoAngle) #create a quaternion for rotation Kd = np.zeros((3, NumSample)) #initialize the planewave list # scaleFactor = np.zeros(NumSample) #initialize a list of scalefactors which are used to scale down the amplitude of planewaves later on along latitude domain #convert the axis from Cartesian to Spherical pha = math.acos(CenterKd[2] / r) #calculate polar angle pha from Z coordinate phaM = math.asin(NA_out / np.real( self.n)) #calculate sample range of pha from numerical aperture inZ = np.cos(pha) #lower boundary of sampling along Z axis outZ = np.cos(phaM) #upper boundary of sampling along Z axis rangeZ = np.abs(inZ) - np.abs(outZ) #sampling range along Z axis # phaStep = phaM / NumSample #set longitudinal sample resolution as maximal pha divided by number of samples # thetaStep = thetaM / NumSample #set latitudinal sample resolution as maximal theta divided by number of samples ###following is uniform sampling # for i in range(NumSample): #sample along longitudinal (pha) domain # for j in range(NumSample): #sample along latitudinal (theta) domain # KdR = r #sample hemisphere radiance will be all the same as r # KdTheta = theta + thetaStep * j #sample theta at each step in the sample range # KdPha = pha + phaStep * i #sample theta at each step in the sample range # Kd[0,j,i] = KdR * np.cos(KdTheta) * np.sin(KdPha) #convert coordinates from spherical to Cartesian # Kd[1,j,i] = KdR * np.sin(KdTheta) * np.sin(KdPha) # Kd[2,j,i] = KdR * np.cos(KdPha) # Kd[:,j,i] = beamRotate.rotate(Kd[:,j,i]) #rotate k vectors by the quaternion generated # scaleFactor[j,i] = np.sin(KdPha) #calculate the scalefactors by the current polar angle pha # ###here comes Monte Carlo Sampling for i in range(NumSample): KdR = r #the r coordinate of the vector under spherical system KdTheta = random.random( ) * 2 * np.pi #get a random value for theta coordinate under spherical system KdZ = random.random( ) * rangeZ + inZ #get a random value for Z coordinate under cartesian system KdPha = math.acos(KdZ) #convert it back to spherical system Kd[0, i] = KdR * np.cos(KdTheta) * np.sin( KdPha) #convert coordinates from spherical to Cartesian Kd[1, i] = KdR * np.sin(KdTheta) * np.sin( KdPha ) #the reason why we sample Z at cartesian is that we want the vectors to distribute randomly on that direction Kd[2, i] = KdR * np.cos( KdPha ) #if we sample it on phi domain, they will be denser towards center Kd[:, i] = beamRotate.rotate( Kd[:, i]) #rotate k vectors by the quaternion generated # Kd = np.reshape(Kd, ((3, NumSample ** 2))) # scaleFactor = np.reshape(scaleFactor, ((NumSample ** 2))) #reshape list of k vectors and scalefactors to an one dimentional list end3 = time.time() print("sample planewaves: " + str(end3 - start3) + " s\n") return Kd
def _fill_trainval_infos(nusc, train_scenes, val_scenes, test=False, max_sweeps=10): train_nusc_infos = [] val_nusc_infos = [] from pyquaternion import Quaternion for sample in prog_bar(nusc.sample): lidar_token = sample["data"]["LIDAR_TOP"] cam_front_token = sample["data"]["CAM_FRONT"] sd_rec = nusc.get('sample_data', sample['data']["LIDAR_TOP"]) # =================== for lidar =================== # {'is_key_frame': True, # 'prev': '', # 'fileformat': 'bin', # 'token': 'ec9950f7b5d4ae85ae48d07786e09cebbf4ee771d054353f1e24a95700b4c4af', # 'timestamp': 1557858039302414.8, # 'next': 'b2fb6b275352ff1bc8d63cae2ec88561dddb044cae6f8e6ee7ada4ed07d79dc7', # 'ego_pose_token': '2d673d4bee560c77788b91e2ee24503538e74a23e7972e3e0099b92015f76dde', # 'sample_token': '24b0962e44420e6322de3f25d9e4e5cc3c7a348ec00bfa69db21517e4ca92cc8', # 'calibrated_sensor_token': '82130f5d48b806b62fec95989081337218fbf338ebcc95115d8afcebb305630c', # 'filename': 'lidar/host-a101_lidar1_1241893239302414726.bin', # 'sensor_modality': 'lidar', # 'channel': 'LIDAR_TOP'} # =================== for camera =================== # {'width': 1920, # 'height': 1080, # 'calibrated_sensor_token': '8e73e320d1fa9e5af96059e6eb1dd7d28e3271dea04de86ead47fa25fd13fd20', # 'token': 'fb40b3b5b9d289cd0e763bec34e327d3317a7b416f787feac0d387363b4d00f0', # 'sample_token': '24b0962e44420e6322de3f25d9e4e5cc3c7a348ec00bfa69db21517e4ca92cc8', # 'is_key_frame': True, # 'prev': '', # 'fileformat': 'jpeg', # 'ego_pose_token': '0c257254dad346c9d90f7970ce2c0b8142f7c6e6a90716f4c0538cd2d2ef77d5', # 'timestamp': 1557858039250000.0, # 'next': '00ba71deb97524b5d9be9b677962b74bb32f89284ad7838462250218786b903e', # 'filename': 'images/host-a101_cam0_1241893239250000006.jpeg', # 'sensor_modality': 'camera', # 'channel': 'CAM_FRONT'} cs_record = nusc.get('calibrated_sensor', sd_rec['calibrated_sensor_token']) pose_record = nusc.get('ego_pose', sd_rec['ego_pose_token']) lidar_path, boxes, _ = nusc.get_sample_data(lidar_token) # PosixPath('/media/zzhou/data/data-lyft-3D-objects/lidar/host-a101_lidar1_1241893239302414726.bin') # boxes list[lyft_dataset_sdk.utils.data_classes.Box] # boxes[3].corners() returns array with shape (3, 8) # (Convention: x points forward, y to the left, z up.) # my_boxes[3].name -> "truck" # my_boxes[3].orientation -> Quaternion(0.7094216708769627, 0.009708448436296943, -0.006909889242425616, -0.7046835405696343) # I assume boxes class is for rendering cam_path, _, cam_intrinsic = nusc.get_sample_data(cam_front_token) # PosixPath('/media/zzhou/data/data-lyft-3D-objects/images/host-a101_cam0_1241893239250000006.jpeg') # 3x3 np.ndarray assert Path(lidar_path).exists(), ( "you must download all trainval data, key-frame only dataset performs far worse than sweeps." ) info = { "lidar_path": lidar_path, "cam_front_path": cam_path, "token": sample["token"], "sweeps": [], "lidar2ego_translation": cs_record['translation'], "lidar2ego_rotation": cs_record['rotation'], "ego2global_translation": pose_record['translation'], "ego2global_rotation": pose_record['rotation'], "timestamp": sample["timestamp"], } l2e_r = info["lidar2ego_rotation"] l2e_t = info["lidar2ego_translation"] e2g_r = info["ego2global_rotation"] e2g_t = info["ego2global_translation"] l2e_r_mat = Quaternion(l2e_r).rotation_matrix e2g_r_mat = Quaternion(e2g_r).rotation_matrix sd_rec = nusc.get('sample_data', sample['data']["LIDAR_TOP"]) sweeps = [] while len(sweeps) < max_sweeps: if not sd_rec['prev'] == "": sd_rec = nusc.get('sample_data', sd_rec['prev']) cs_record = nusc.get('calibrated_sensor', sd_rec['calibrated_sensor_token']) pose_record = nusc.get('ego_pose', sd_rec['ego_pose_token']) lidar_path = nusc.get_sample_data_path(sd_rec['token']) sweep = { "lidar_path": lidar_path, "sample_data_token": sd_rec['token'], "lidar2ego_translation": cs_record['translation'], "lidar2ego_rotation": cs_record['rotation'], "ego2global_translation": pose_record['translation'], "ego2global_rotation": pose_record['rotation'], "timestamp": sd_rec["timestamp"] } l2e_r_s = sweep["lidar2ego_rotation"] l2e_t_s = sweep["lidar2ego_translation"] e2g_r_s = sweep["ego2global_rotation"] e2g_t_s = sweep["ego2global_translation"] # sweep->ego->global->ego'->lidar l2e_r_s_mat = Quaternion(l2e_r_s).rotation_matrix e2g_r_s_mat = Quaternion(e2g_r_s).rotation_matrix R = (l2e_r_s_mat.T @ e2g_r_s_mat.T) @ ( np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T) T = (l2e_t_s @ e2g_r_s_mat.T + e2g_t_s) @ ( np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T) T -= e2g_t @ (np.linalg.inv(e2g_r_mat).T @ np.linalg.inv( l2e_r_mat).T) + l2e_t @ np.linalg.inv(l2e_r_mat).T sweep["sweep2lidar_rotation"] = R.T # points @ R.T + T sweep["sweep2lidar_translation"] = T sweeps.append(sweep) else: break info["sweeps"] = sweeps if not test: annotations = [ nusc.get('sample_annotation', token) for token in sample['anns'] ] locs = np.array([b.center for b in boxes]).reshape( -1, 3) # w.r.t global coordinates, use lidar coord ? dims = np.array([b.wlh for b in boxes]).reshape(-1, 3) rots = np.array([b.orientation.yaw_pitch_roll[0] for b in boxes]).reshape(-1, 1) velocity = np.array( [nusc.box_velocity(token)[:2] for token in sample['anns']]) # convert velo from global to lidar for i in range(len(boxes)): velo = np.array([*velocity[i], 0.0]) velo = velo @ np.linalg.inv(e2g_r_mat).T @ np.linalg.inv( l2e_r_mat).T velocity[i] = velo[:2] names = [b.name for b in boxes] for i in range(len(names)): if names[i] in NuScenesDataset.NameMapping: names[i] = NuScenesDataset.NameMapping[names[i]] names = np.array(names) # we need to convert rot to SECOND format. # change the rot format will break all checkpoint, so... gt_boxes = np.concatenate([locs, dims, -rots - np.pi / 2], axis=1) assert len(gt_boxes) == len( annotations), f"{len(gt_boxes)}, {len(annotations)}" info["gt_boxes"] = gt_boxes info["gt_names"] = names info["gt_velocity"] = velocity.reshape(-1, 2) info["num_lidar_pts"] = np.array( [a["num_lidar_pts"] for a in annotations]) info["num_radar_pts"] = np.array( [a["num_radar_pts"] for a in annotations]) if sample["scene_token"] in train_scenes: train_nusc_infos.append(info) else: val_nusc_infos.append(info) return train_nusc_infos, val_nusc_infos
The distance between the poses in y-direction Returns ------ List[Pose] A list of generated poses """ # The point defines the rotation and is a corner of the grid rotation = pose.quaternion # Calculate the delta of each step in x- and y-direction # For that, we scale the unit vector pointing in x-direction/y-direction # and then rotate it by the rotation of the pose delta_x = rotation.rotate(np.array([xStepSize, 0.0, 0.0])) delta_y = rotation.rotate(np.array([0.0, yStepSize, 0.0])) poses = [] # type: List[Pose] for y_index in range(ySize): for x_index in range(ySize): translation = pose.translation + x_index * delta_x + y_index * delta_y poses.append(Pose(translation, rotation)) return poses if __name__ == '__main__': # Called when running this script standalone translation = [0.6089578, 0.3406782, 0.208865] rotation = Quaternion(w=0.231852, x=0.33222, y=0.746109, z=0.528387) pose = Pose(translation, rotation) poses = main(pose, 3, 3, 0.05, 0.05) print(poses)
def _fill_train_infos(lyft, train_scenes, test=False, max_sweeps=10): train_lyft_infos = [] from pyquaternion import Quaternion print("sample number:", len(lyft.sample)) for sample in prog_bar(lyft.sample): lidar_token = sample["data"]["LIDAR_TOP"] cam_front_token = sample["data"]["CAM_FRONT"] sd_rec = lyft.get('sample_data', sample['data']["LIDAR_TOP"]) cs_record = lyft.get('calibrated_sensor', sd_rec['calibrated_sensor_token']) pose_record = lyft.get('ego_pose', sd_rec['ego_pose_token']) lidar_path, boxes, _ = lyft.get_sample_data(lidar_token) cam_path, _, cam_intrinsic = lyft.get_sample_data(cam_front_token) assert Path(lidar_path).exists() info = { "lidar_path": lidar_path, "cam_front_path": cam_path, "token": sample["token"], "sweeps": [], "lidar2ego_translation": cs_record['translation'], "lidar2ego_rotation": cs_record['rotation'], "ego2global_translation": pose_record['translation'], "ego2global_rotation": pose_record['rotation'], "timestamp": sample["timestamp"], } # print("info:", info) l2e_r = info["lidar2ego_rotation"] l2e_t = info["lidar2ego_translation"] e2g_r = info["ego2global_rotation"] e2g_t = info["ego2global_translation"] l2e_r_mat = Quaternion(l2e_r).rotation_matrix e2g_r_mat = Quaternion(e2g_r).rotation_matrix sd_rec = lyft.get('sample_data', sample['data']["LIDAR_TOP"]) sweeps = [] while len(sweeps) < max_sweeps: if not sd_rec['prev'] == "": sd_rec = lyft.get('sample_data', sd_rec['prev']) cs_record = lyft.get('calibrated_sensor', sd_rec['calibrated_sensor_token']) pose_record = lyft.get('ego_pose', sd_rec['ego_pose_token']) lidar_path = lyft.get_sample_data_path(sd_rec['token']) sweep = { "lidar_path": lidar_path, "sample_data_token": sd_rec['token'], "lidar2ego_translation": cs_record['translation'], "lidar2ego_rotation": cs_record['rotation'], "ego2global_translation": pose_record['translation'], "ego2global_rotation": pose_record['rotation'], "timestamp": sd_rec["timestamp"] } l2e_r_s = sweep["lidar2ego_rotation"] l2e_t_s = sweep["lidar2ego_translation"] e2g_r_s = sweep["ego2global_rotation"] e2g_t_s = sweep["ego2global_translation"] # sweep->ego->global->ego'->lidar l2e_r_s_mat = Quaternion(l2e_r_s).rotation_matrix e2g_r_s_mat = Quaternion(e2g_r_s).rotation_matrix R = (l2e_r_s_mat.T @ e2g_r_s_mat.T) @ ( np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T) T = (l2e_t_s @ e2g_r_s_mat.T + e2g_t_s) @ ( np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T) T -= e2g_t @ (np.linalg.inv(e2g_r_mat).T @ np.linalg.inv( l2e_r_mat).T) + l2e_t @ np.linalg.inv(l2e_r_mat).T sweep["sweep2lidar_rotation"] = R.T # points @ R.T + T sweep["sweep2lidar_translation"] = T sweeps.append(sweep) else: break info["sweeps"] = sweeps # print("sweeps:", sweeps) if not test: annotations = [ lyft.get('sample_annotation', token) for token in sample['anns'] ] locs = np.array([b.center for b in boxes]).reshape(-1, 3) dims = np.array([b.wlh for b in boxes]).reshape(-1, 3) rots = np.array([b.orientation.yaw_pitch_roll[0] for b in boxes]).reshape(-1, 1) velocity = np.array( [lyft.box_velocity(token)[:2] for token in sample['anns']]) # convert velo from global to lidar for i in range(len(boxes)): velo = np.array([*velocity[i], 0.0]) velo = velo @ np.linalg.inv(e2g_r_mat).T @ np.linalg.inv( l2e_r_mat).T velocity[i] = velo[:2] names = [b.name for b in boxes] for i in range(len(names)): if names[i] in LyftDataset.NameMapping: names[i] = LyftDataset.NameMapping[names[i]] names = np.array(names) # we need to convert rot to SECOND format. # change the rot format will break all checkpoint, so... gt_boxes = np.concatenate([locs, dims, -rots - np.pi / 2], axis=1) assert len(gt_boxes) == len( annotations), f"{len(gt_boxes)}, {len(annotations)}" info["gt_boxes"] = gt_boxes info["gt_names"] = names info["gt_velocity"] = velocity.reshape(-1, 2) info["num_lidar_pts"] = np.array( [a["num_lidar_pts"] for a in annotations]) info["num_radar_pts"] = np.array( [a["num_radar_pts"] for a in annotations]) if sample["scene_token"] in train_scenes: # print("sample_scene_token:", sample["scene_token"]) train_lyft_infos.append(info) # else: # val_nusc_infos.append(info) return train_lyft_infos
def parse_sample(sample_token, nusc): ret_dict = {} sample_data = nusc.get('sample', sample_token) timestamp = sample_data['timestamp'] # First Step: Get lidar points in global frame cord! lidar_token = sample_data['data']['LIDAR_TOP'] lidar_data = nusc.get('sample_data', lidar_token) pcl_path = osp.join(nusc.dataroot, lidar_data['filename']) pc = LidarPointCloud.from_file(pcl_path) # lidar point in point sensor frame cs_record = nusc.get('calibrated_sensor', lidar_data['calibrated_sensor_token']) pc.rotate(Quaternion(cs_record['rotation']).rotation_matrix) pc.translate(np.array(cs_record['translation'])) # Second step: transform from ego to the global frame. poserecord = nusc.get('ego_pose', lidar_data['ego_pose_token']) pc.rotate(Quaternion(poserecord['rotation']).rotation_matrix) pc.translate(np.array(poserecord['translation'])) # First Step finished! pc in global frame for cam_name in CamNames: cam_token = sample_data['data'][cam_name] _, boxes, _ = nusc.get_sample_data(cam_token, box_vis_level=BoxVisibility.ANY) all_points_next = [] all_sf_next = [] all_points_prev = [] all_sf_prev = [] for box in boxes: ann_token = box.token points_next, sf_next, points_prev, sf_prev, _ = get_sf_ann(ann_token, timestamp, deepcopy(pc.points[:3, ]), nusc) if points_next is not None: all_points_next.append(points_next) all_sf_next.append(sf_next) if points_prev is not None: all_points_prev.append(points_prev) all_sf_prev.append(sf_prev) if len(all_points_next) > 0: points_next = np.concatenate(all_points_next, axis=-1) sf_next = np.concatenate(all_sf_next, axis=-1) else: points_next = None sf_next = None if len(all_points_prev) > 0: points_prev = np.concatenate(all_points_prev, axis=-1) sf_prev = np.concatenate(all_sf_prev, axis=-1) else: points_prev = None sf_prev = None # transform points to ego pose; change sf's orentiation # change from global to ego pose ret_points_list = [] for points, sf in zip([points_next, points_prev], [sf_next, sf_prev]): if points is None: ret_points_list.append(None) continue points = points - np.array(poserecord['translation'])[:, np.newaxis] points = np.dot(Quaternion(poserecord['rotation']).rotation_matrix.T, points) sf = np.dot(Quaternion(poserecord['rotation']).rotation_matrix.T, sf) # change from ego to camera cam_data = nusc.get('sample_data', cam_token) cam_cs = nusc.get('calibrated_sensor', cam_data['calibrated_sensor_token']) # transform points for ego pose to camera pose cam_points = deepcopy(points) - np.array(cam_cs['translation'])[:, np.newaxis] cam_points = np.dot(Quaternion(cam_cs['rotation']).rotation_matrix.T, cam_points) cam_sf = np.dot(Quaternion(cam_cs['rotation']).rotation_matrix.T, sf) ret_points = np.concatenate([cam_points, cam_sf], axis=0) ret_points_list.append(ret_points) ret_dict[cam_name] = ret_points_list return ret_dict
def test_unequal_quaternions(self): q1 = Quaternion(1, 0, 0, 0) q2 = Quaternion(0, 1, 0, 0) self.assertNotEqual(hash(q1), hash(q2))
def transform_vector(vec, axis, angle): q = Quaternion(axis=axis, degrees=angle) vec_ = np.zeros((4)) vec_[1:] = vec vec = q * vec_ * q.inverse return vec.elements[1:].tolist()
def test_init_from_explicit_component(self): a, b, c, d = randomElements() # Using 'real' & 'imaginary' notation q1 = Quaternion(real=a, imaginary=(b, c, d)) q2 = Quaternion(real=a, imaginary=[b, c, d]) q3 = Quaternion(real=a, imaginary=np.array([b, c, d])) q4 = Quaternion(real=a) q5 = Quaternion(imaginary=np.array([b, c, d])) q6 = Quaternion(real=None, imaginary=np.array([b, c, d])) self.assertIsInstance(q1, Quaternion) self.assertIsInstance(q2, Quaternion) self.assertIsInstance(q3, Quaternion) self.assertIsInstance(q4, Quaternion) self.assertIsInstance(q5, Quaternion) self.assertIsInstance(q6, Quaternion) self.assertEqual(q1, Quaternion(a, b, c, d)) self.assertEqual(q1, q2) self.assertEqual(q2, q3) self.assertEqual(q4, Quaternion(a, 0, 0, 0)) self.assertEqual(q5, Quaternion(0, b, c, d)) self.assertEqual(q5, q6) with self.assertRaises(ValueError): q = Quaternion(real=a, imaginary=[b, c]) with self.assertRaises(ValueError): q = Quaternion(real=a, imaginary=(b, c, d, d)) # Using 'scalar' & 'vector' notation q1 = Quaternion(scalar=a, vector=(b, c, d)) q2 = Quaternion(scalar=a, vector=[b, c, d]) q3 = Quaternion(scalar=a, vector=np.array([b, c, d])) q4 = Quaternion(scalar=a) q5 = Quaternion(vector=np.array([b, c, d])) q6 = Quaternion(scalar=None, vector=np.array([b, c, d])) self.assertIsInstance(q1, Quaternion) self.assertIsInstance(q2, Quaternion) self.assertIsInstance(q3, Quaternion) self.assertIsInstance(q4, Quaternion) self.assertIsInstance(q5, Quaternion) self.assertIsInstance(q6, Quaternion) self.assertEqual(q1, Quaternion(a, b, c, d)) self.assertEqual(q1, q2) self.assertEqual(q2, q3) self.assertEqual(q4, Quaternion(a, 0, 0, 0)) self.assertEqual(q5, Quaternion(0, b, c, d)) self.assertEqual(q5, q6) with self.assertRaises(ValueError): q = Quaternion(scalar=a, vector=[b, c]) with self.assertRaises(ValueError): q = Quaternion(scalar=a, vector=(b, c, d, d))
def calculateOrientation(x, im_width, fov_w): angle = float(x - im_width / 2.0) / im_width * fov_w q = Quaternion(axis=[0, 0, 1], radians=angle) return q, angle
def test_str(self): a, b, c, d = randomElements() q = Quaternion(a, b, c, d) string = "{:.3f} {:+.3f}i {:+.3f}j {:+.3f}k".format(a, b, c, d) self.assertEqual(string, str(q))
br = tf.TransformBroadcaster() while not rospy.is_shutdown(): try: (trans_vel_baselink, rot_vel_baselink) = t.lookupTransform('/velodyne_sensor', '/base_link', rospy.Time(0)) (trans_world_vel, rot_world_vel) = t.lookupTransform('/map', '/velodyne', rospy.Time(0)) H_vel_baselink = create_H(trans_vel_baselink, rot_vel_baselink) H_world_vel = create_H(trans_world_vel, rot_world_vel) H_world_baselink = np.matmul(H_world_vel, H_vel_baselink) q = Quaternion(matrix=H_world_baselink).elements trans_world_baselink = (H_world_baselink[0, 3], H_world_baselink[1, 3], H_world_baselink[2, 3]) rot_world_baselink = (q[1], q[2], q[3], q[0]) br.sendTransform(trans_world_baselink, rot_world_baselink, rospy.Time.now(), "/base_link", "/map") # br.sendTransform((0,0,0), (1,0,0,0), rospy.Time(0), "/world", "/map") except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logerr("TF lookup error") continue
def test_bool(self): self.assertTrue(Quaternion()) self.assertFalse(Quaternion(scalar=0.0)) self.assertTrue(~Quaternion(scalar=0.0)) self.assertFalse(~Quaternion())
def _add_gt_annotations_Car3d(self, idx, image_shape, im_scale): """Add ground truth annotation metadata to an roidb entry.""" morph_ellipse_kernel = cv2.getStructuringElement( cv2.MORPH_ELLIPSE, (5, 5)) # initiate the lists masks = [] segms = [] boxes = [] car_cat_classes = [] poses = [] quaternions = [] car_pose_file = os.path.join(self.dataset_dir, 'car_poses', self.img_list_all[idx] + '.json') assert os.path.exists(car_pose_file), 'Label \'{}\' not found'.format( car_pose_file) f = open(car_pose_file, 'r') data = f.read() f.close() car_poses = json.loads(data) for i, car_pose in enumerate(car_poses): car_name = self.car_id2name[car_pose['car_id']].name car = self.car_models[car_name] pose = np.array(car_pose['pose']) # Fix for kaggle pku-autonomous-driving yaw, pitch, roll = pose[:3] yaw, pitch, roll = -pitch, -yaw, -roll pose[:3] = yaw, pitch, roll # project 3D points to 2d image plane rot_mat = euler_angles_to_rotation_matrix(pose[:3]) rvect, _ = cv2.Rodrigues(rot_mat) imgpts, jac = cv2.projectPoints(np.float32(car['vertices']), rvect, pose[3:], self.intrinsic_mat, distCoeffs=None) imgpts = np.int32(imgpts).reshape(-1, 2) # project 3D points to 2d image plane mask = np.zeros((image_shape[1], image_shape[0])) for face in car['faces'] - 1: pts = np.array([[imgpts[idx, 0], imgpts[idx, 1]] for idx in face], np.int32) pts = pts.reshape((-1, 1, 2)) #cv2.polylines(mask, [pts], True, (255, 255, 255)) cv2.drawContours(mask, [pts], 0, (255, 255, 255), -1) # Find mask ground_truth_binary_mask = np.zeros(mask.shape, dtype=np.uint8) ground_truth_binary_mask[mask == 255] = 1 if self.cfg['INPUT']['BOTTOM_HALF']: ground_truth_binary_mask = ground_truth_binary_mask[ int(mask.shape[0] / 2):, :] ground_truth_binary_mask = cv2.morphologyEx( ground_truth_binary_mask, cv2.MORPH_CLOSE, morph_ellipse_kernel) fortran_ground_truth_binary_mask = np.asfortranarray( ground_truth_binary_mask) encoded_ground_truth = maskUtils.encode( fortran_ground_truth_binary_mask) contours = measure.find_contours( np.array(ground_truth_binary_mask), 0.5) mask_instance = [] # if len(contours) > 1: # print("Image with problem: %d, %s" % (idx, self.img_list_all[idx])) for contour in contours: contour = np.flip(contour, axis=1) segmentation = contour.ravel().tolist() mask_instance.append(segmentation) masks.append(mask_instance) segms.append(encoded_ground_truth) #cv2.imwrite(os.path.join('/media/SSD_1TB/ApolloScape', self.img_list_all[idx] + '_' + str(i) + '_.jpg'), mask) x1, y1, x2, y2 = imgpts[:, 0].min(), imgpts[:, 1].min( ), imgpts[:, 0].max(), imgpts[:, 1].max() if self.cfg['INPUT']['BOTTOM_HALF']: y1 -= int(image_shape[1] / 2) y2 -= int(image_shape[1] / 2) boxes.append([x1, y1, x2, y2]) #q = euler_angles_to_quaternions(np.array(car_pose['pose'][:3]))[0] q = Quaternion(matrix=rot_mat).q if self.cfg['MODEL']['ROI_CAR_CLS_ROT_HEAD'][ 'QUATERNION_HEMISPHERE']: q = quaternion_upper_hemispher(q) quaternions.append(q) car_cat_classes.append( np.where(self.unique_car_models == car_pose['car_id'])[0][0]) poses.append(car_pose['pose']) if self.cfg['INPUT']['BOTTOM_HALF']: target = BoxList(boxes, (image_shape[0], int(image_shape[1] / 2)), mode="xyxy") masks = SegmentationMask(masks, (image_shape[0], int(image_shape[1] / 2))) else: target = BoxList(boxes, image_shape, mode="xyxy") masks = SegmentationMask(masks, image_shape) car_cat_classes = torch.tensor(car_cat_classes) target.add_field('car_cat_classes', car_cat_classes) target.add_field("masks", masks) quaternions = torch.tensor(quaternions) target.add_field("quaternions", quaternions) poses = torch.tensor(poses) target.add_field("poses", poses) labels = np.ones(car_cat_classes.shape) labels = torch.tensor(labels) target.add_field("labels", labels) im_scales = im_scale * np.ones(car_cat_classes.shape) im_scales = torch.tensor(im_scales) target.add_field("im_scales", im_scales) target = target.clip_to_image(remove_empty=True) # for evaluation purpose if not self.training: target.add_field("segms", segms) return target
def test_complex(self): a, b, c, d = randomElements() q = Quaternion(a, b, c, d) self.assertEqual(complex(q), complex(a, b))
from Lib import * import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import axes3d from pyquaternion import Quaternion v = [0, 0, 1] axis = [0, 1, 0] theta = -np.pi / 2 # radian vX, vY, vZ = Quaternion(axis=axis, angle=theta).rotate(v) print("rotate Vector : {}".format(vX)) fig = plt.figure() ax = fig.add_subplot(2, 1, 1, projection='3d') ax.quiver(0, 0, 0, v[0], v[1], v[2], length=1, normalize=True) ax.quiver(0, 0, 0, vX, vY, vZ, length=1, normalize=True) ax.set_xlabel("x axis") ax.set_ylabel("y axis") ax.set_zlabel("z axis") ax.set_xlim(-3, 3) ax.set_ylim(-3, 3) ax.set_zlim(-3, 3) plt.show() # print(np.dot(rotation_matrix(axis,theta), v))
def test_unary_minus(self): a, b, c, d = randomElements() q = Quaternion(a, b, c, d) self.assertEqual(-q, Quaternion(-a, -b, -c, -d))
def get_relative_position(self): if self.tracking_mode == "OSC_direct": try: angles = self.osc_input_server.get_current_angle() except: return self.fallback_angle[0], self.fallback_angle[ 1], self.fallback_angle[2] return angles[0], angles[1], angles[2] try: if self.acoustical_center_pos is not None: pose_head, _ = self.get_tracker_data(only_tracker_1=True) translation_speaker = self.acoustical_center_pos else: # if speaker is not calibrated yet, the live tracking speaker pose is used pose_head, pose_speaker = self.get_tracker_data() translation_speaker = np.array([ pose_speaker.m[0][3], pose_speaker.m[1][3], pose_speaker.m[2][3] ]) except: return self.fallback_angle[0], self.fallback_angle[ 1], self.fallback_angle[2] if pose_head != False: # MYSTERIOUS PROBLEM: The Y and Z Axis are flipped in the TrackerPose from openVR most of the times. mystery_flag = True #for testing debugging # STEP1: get the correct translation between head and speaker translation_head = np.array( [pose_head.m[0][3], pose_head.m[1][3], pose_head.m[2][3]]) if self.head_dimensions['ear_center'] is not None: offset_x = self.head_dimensions['ear_center'][0] * np.array( [pose_head.m[0][0], pose_head.m[1][0], pose_head.m[2][0]]) offset_y = self.head_dimensions['ear_center'][1] * np.array( [pose_head.m[0][1], pose_head.m[1][1], pose_head.m[2][1]]) offset_z = self.head_dimensions['ear_center'][2] * np.array( [pose_head.m[0][2], pose_head.m[1][2], pose_head.m[2][2]]) translation_head = translation_head + offset_x + offset_y + offset_z # get vector pointing from head center to speaker center transvec = translation_speaker - translation_head # STEP2: get the correct orientation of the head # get the base tracker rotation in quaternions fwd = np.array( [pose_head.m[0][2], pose_head.m[1][2], pose_head.m[2][2]]) up = np.array( [pose_head.m[0][1], pose_head.m[1][1], pose_head.m[2][1]]) side = np.array( [pose_head.m[0][0], pose_head.m[1][0], pose_head.m[2][0]]) r_w = np.sqrt(abs(1 + side[0] + up[1] + fwd[2])) / 2 r_x = (up[2] - fwd[1]) / (4 * r_w) r_y = (fwd[0] - side[2]) / (4 * r_w) r_z = (side[1] - up[0]) / (4 * r_w) head_rotation = Quaternion(r_w, r_x, r_y, r_z) # apply calibrated rotation rotation = head_rotation * self.calibrationRotation # make "new' direction vectors by rotating a normed set of orthogonal direction vectors if mystery_flag: side = np.array([1.0, 0.0, 0.0]) up = np.array([ 0.0, 0.0, -1.0 ]) # i don´t know why, but y and z axis are flipped somehow fwd = np.array([0.0, 1.0, 0.0]) # else: side = np.array([1.0, 0.0, 0.0]) up = np.array([0.0, 1.0, 0.0]) fwd = np.array([0.0, 0.0, 1.0]) side = rotation.rotate(side) up = rotation.rotate(up) fwd = rotation.rotate(fwd) # STEP3: calculate direction vector to speaker relative to head coordinate system # (head coordinate system = side, up, fwd) direction_vector = np.array([ np.inner(side, transvec), np.inner(up, transvec), np.inner(fwd, transvec) ]) radius = np.linalg.norm(direction_vector) direction_vector = direction_vector / radius # get spherical coordinates from direction vector az = np.rad2deg( np.arctan2(-direction_vector[0], -direction_vector[2])) if az < 0: az += 360 el = np.rad2deg(np.arccos(-direction_vector[1])) el = el - 90 #print(az, el, radius) return az, el, radius
def test_is_unit(self): q1 = Quaternion() q2 = Quaternion(1.0, 0, 0, 0.0001) self.assertTrue(q1.is_unit()) self.assertFalse(q2.is_unit()) self.assertTrue(q2.is_unit(0.001))
def __init__(self): self.fallback_angle = np.array([0.0, 0.0, 1.0]) self.trackers_switched = False self.counter = 0 self.head_dimensions = { 'ear_pos_l': None, 'ear_pos_r': None, 'ear_center': None, 'left_pos': None, 'right_pos': None, 'head_width': None, 'front_pos': None, 'back_pos': None, 'head_length': None } self.tracking_mode = "Vive" self.osc_input_server = osc_input.OSCInputServer() self.osc_input_server.start_listening() self.acoustical_center_pos = None self.vr_system_initialized = False # initialize open VR try: self.vr_system = init(VRApplication_Background) except: return self.vr_system_initialized = True # get available trackers and controllers for deviceID in range(k_unMaxTrackedDeviceCount): dev_class = self.vr_system.getTrackedDeviceClass(deviceID) if (dev_class == TrackedDeviceClass_GenericTracker): if (not hasattr(self, 'tracker1')): if self.vr_system.isTrackedDeviceConnected(deviceID): self.tracker1 = Device( TrackedDeviceClass_GenericTracker, deviceID) self.tracker1.set_availability(True) elif (not hasattr(self, 'tracker2')): if self.vr_system.isTrackedDeviceConnected(deviceID): self.tracker2 = Device( TrackedDeviceClass_GenericTracker, deviceID) self.tracker2.set_availability(True) elif (dev_class == TrackedDeviceClass_Controller): if (not hasattr(self, 'controller1')): self.controller1 = Device(TrackedDeviceClass_Controller, deviceID) self.controller1.set_availability( self.vr_system.isTrackedDeviceConnected(deviceID)) elif (not hasattr(self, 'controller2')): self.controller2 = Device(TrackedDeviceClass_Controller, deviceID) self.controller2.set_availability( self.vr_system.isTrackedDeviceConnected(deviceID)) # if(hasattr(self, 'tracker1')): # #print("Has tracker one!") # if(self.tracker1.is_available()): # pass # #print("Tracker 1 Available") # # if (hasattr(self, 'tracker2')): # #print("Has tracker two!") # if (self.tracker2.is_available()): # pass # #print("Tracker 2 Available") # # if (hasattr(self, 'controller1')): # #print("Has controller 1!") # pass # # if (hasattr(self, 'controller2')): # #print("Has controller 2!") # pass self.calibrationRotation = Quaternion() self.calibrate_orientation()
def test_exp(self): from math import exp q = Quaternion(axis=[1,0,0], angle=pi) exp_q = Quaternion.exp(q) self.assertEqual(exp_q, exp(0) * Quaternion(scalar=cos(1.0), vector=[sin(1.0), 0,0]))
def plot_road_agents(self, ax, fig, sample, plot_list, text_box, sensor_info, my_patch, plot_traj=False, contour_func=None, animated_agent=False): road_agents_in_patch = {'pedestrians': [], 'vehicles': []} for ann_token in sample['anns']: ann = self.nusc.get('sample_annotation', ann_token) category = ann['category_name'] if len(ann['attribute_tokens']) != 0: attribute = self.nusc.get('attribute', ann['attribute_tokens'][0])['name'] else: attribute = "" pos = [ann['translation'][0], ann['translation'][1]] instance_token = ann['instance_token'] sample_token = sample['token'] #### Plot other agents #### valid_agent = False if 'other_cars' in plot_list and 'vehicle' in category and 'parked' not in attribute and self.in_my_patch( pos, my_patch): valid_agent = True agent_yaw = Quaternion(ann['rotation']) agent_yaw = quaternion_yaw(agent_yaw) agent_yaw = angle_of_rotation(agent_yaw) agent_yaw = np.rad2deg(agent_yaw) self.plot_elements(pos, agent_yaw, 'other_cars', ax, animated_agent=animated_agent) car_info = { 'instance_token': ann['instance_token'], 'category': category, 'attribute': attribute, 'translation': pos, 'rotation_quat': ann['rotation'], 'rotation_deg': agent_yaw } road_agents_in_patch['vehicles'].append(car_info) if text_box: self.plot_text_box( ax, category, [ann['translation'][0] + 1.2, ann['translation'][1]]) self.plot_text_box(ax, attribute, [ ann['translation'][0] + 1.2, ann['translation'][1] - 1.2 ]) self.plot_text_box(ax, ann['instance_token'], [ ann['translation'][0] + 1.2, ann['translation'][1] + 1.2 ]) #### Plot pedestrians #### if 'pedestrian' in plot_list and 'pedestrian' in category and not 'stroller' in category and not 'wheelchair' in category and self.in_my_patch( pos, my_patch): valid_agent = True agent_yaw = Quaternion(ann['rotation']) agent_yaw = quaternion_yaw(agent_yaw) agent_yaw = angle_of_rotation(agent_yaw) agent_yaw = np.rad2deg(agent_yaw) self.plot_elements(pos, agent_yaw, 'pedestrian', ax) if text_box: self.plot_text_box( ax, category, [ann['translation'][0] + 1.2, ann['translation'][1]]) self.plot_text_box(ax, attribute, [ ann['translation'][0] + 1.2, ann['translation'][1] - 1.2 ]) if valid_agent and plot_traj: agent_future = self.helper.get_future_for_agent( instance_token, sample_token, self.na_config['pred_horizon'], in_agent_frame=False, just_xy=True) agent_past = self.helper.get_past_for_agent( instance_token, sample_token, self.na_config['obs_horizon'], in_agent_frame=False, just_xy=True) if len(agent_future) > 0: ax.scatter(agent_future[:, 0], agent_future[:, 1], s=10, c='y', alpha=1.0, zorder=200) if len(agent_past) > 0: ax.scatter(agent_past[:, 0], agent_past[:, 1], s=10, c='k', alpha=0.2, zorder=200) return road_agents_in_patch