def _load_mvbbs_simulated(self): try: f = open(self.filename_simulated) reader = csv.reader(f) self.db_simulated = {} for row in reader: object_dims = tuple( [float(v) for i, v in enumerate(row) if i in range(3)]) t = [float(v) for i, v in enumerate(row) if i in range(3, 6)] q = [float(row[9])] + [ float(v) for i, v in enumerate(row) if i in range(6, 9) ] T = np.array(se3.homogeneous((so3.from_quaternion(q), t))) t = [float(v) for i, v in enumerate(row) if i in range(10, 13)] q = [float(row[16])] + [ float(v) for i, v in enumerate(row) if i in range(13, 16) ] h_T_o = np.array(se3.homogeneous((so3.from_quaternion(q), t))) q_hand = [ float(v) for i, v in enumerate(row) if i in range(17, 17 + self.n_dofs) ] c_p = [ float(v) for i, v in enumerate(row) if i in range(17 + self.n_dofs, 17 + self.n_dofs + 3 * self.n_l) ] c_f = [ float(v) for i, v in enumerate(row) if i in range(17 + self.n_dofs + 3 * self.n_l, 17 + self.n_dofs + 9 * self.n_l) ] if tuple(object_dims) not in self.db_simulated: self.db_simulated[tuple(object_dims)] = [] obj_pose_q_c_p_c_f = { 'T': T, 'h_T_o': h_T_o, 'q': q_hand, 'c_p': c_p, 'c_f': c_f } self.db_simulated[tuple(object_dims)].append( obj_pose_q_c_p_c_f) except: pass
def so3_grid(N): """Returns a nx.Graph describing a grid on SO3 with resolution N. The resulting graph has ~4N^3 nodes The node attribute 'params' gives the so3 element. """ assert N >= 1 G = nx.Graph() R0 = so3.from_quaternion(vectorops.unit((1, 1, 1, 1))) namemap = dict() for ax in range(4): for i in xrange(N + 1): u = 2 * float(i) / N - 1.0 u = math.tan(u * math.pi * 0.25) for j in xrange(N + 1): v = 2 * float(j) / N - 1.0 v = math.tan(v * math.pi * 0.25) for k in xrange(N + 1): w = 2 * float(k) / N - 1.0 w = math.tan(w * math.pi * 0.25) idx = [i, j, k] idx = idx[:ax] + [N] + idx[ax:] idx = tuple(idx) if idx in namemap: continue else: namemap[idx] = idx flipidx = tuple([N - x for x in idx]) namemap[flipidx] = idx q = [u, v, w] q = q[:ax] + [1.0] + q[ax:] #print idx,"->",q q = vectorops.unit(q) R = so3.mul(so3.inv(R0), so3.from_quaternion(q)) G.add_node(idx, params=R) for ax in range(4): for i in xrange(N + 1): for j in xrange(N + 1): for k in xrange(N + 1): baseidx = [i, j, k] idx = baseidx[:ax] + [N] + baseidx[ax:] idx = tuple(idx) for n in range(3): nidx = baseidx[:] nidx[n] += 1 if nidx[n] > N: continue nidx = nidx[:ax] + [N] + nidx[ax:] nidx = tuple(nidx) G.add_edge(namemap[idx], namemap[nidx]) return G
def load_rgbd_dataset(dataset): """Returns a list of RGBDScans from a folder""" ground_truth_t,ground_truth_transforms = _load_time_stamped_file(dataset+'/groundtruth.txt') ground_truth_transforms = [[float(v) for v in T] for T in ground_truth_transforms] #convert to Klamp't SE3 elements for i,trans_quat in enumerate(ground_truth_transforms): qx,qy,qz,qw = trans_quat[3:] ground_truth_transforms[i] = (so3.from_quaternion((qw,qx,qy,qz)),trans_quat[:3]) ground_truth_path = SE3Trajectory(ground_truth_t,ground_truth_transforms) rgb_t,rgb_files = _load_time_stamped_file(dataset+'/rgb.txt') depth_t,depth_files = _load_time_stamped_file(dataset+'/depth.txt') depth_t = np.array(depth_t) scans = [] last_ind = -1 for t,file in zip(rgb_t,rgb_files): im = Image.open(dataset+'/'+file) rgb_array = np.asarray(im) ind = np.abs(depth_t-t).argmin() if ind > last_ind+1: ind = last_ind+1 last_ind = ind #print("Time",t,"Depth index",ind,"Distance",np.abs(depth_t-t)[ind]) depth_file = depth_files[ind] im = Image.open(dataset+'/'+depth_file) depth_array = np.asarray(im) T = ground_truth_path.eval_se3(t) scans.append(RGBDScan(rgb_array,depth_array,timestamp=t,T_ground_truth=T)) scans[-1].camera = kinect_factory_calib return scans
def test_rpy(self): R = so3.from_quaternion( (-4.32978e-17, -0.707107, 4.32978e-17, 0.707107)) r, p, y = so3.rpy(R) self.assertAlmostEqual(r, 0.0) self.assertAlmostEqual(p, 1.5707963267948966) self.assertAlmostEqual(y, 3.141592653589793)
def spherical_linear_interpolation_fromR(R0, Rf, h): # See https://en.wikipedia.org/wiki/Slerp q0 = np.array(so3.quaternion(R0)) qf = np.array(so3.quaternion(Rf)) v0 = np.array(MathUtils.normalize(q0)) vf = np.array(MathUtils.normalize(qf)) dot = np.dot(v0, vf) if dot < 0.0: vf = -vf dot = -dot # TODO: Is this causing non rotation matrixes? dot_threshold = 0.995 if dot > dot_threshold: # print(f"WARNING: dot={dot} > threshold={dot_threshold}\t linearly interprolating") # If results are close, calculate new result linearly result = v0 + h * (vf - v0) result_as_list = result.tolist() q = MathUtils.normalize(result_as_list) else: theta_0 = np.arccos(dot) theta = theta_0 * h sin_theta = np.sin(theta) sin_theta_0 = np.sin(theta_0) s0 = np.cos(theta) - dot * sin_theta / sin_theta_0 s1 = sin_theta / sin_theta_0 q = (s0 * v0) + (s1 * vf) return so3.from_quaternion(q)
def rand_rotation(): q = [ random.gauss(0, 1), random.gauss(0, 1), random.gauss(0, 1), random.gauss(0, 1) ] q = vectorops.unit(q) return so3.from_quaternion(q)
def do_lookup(frame,parent): try: (trans,rot) = listener.lookupTransform(frame, parent, rospy.Time(0)) return (so3.from_quaternion((rot[3],rot[0],rot[1],rot[2])),trans) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): if onerror == 'print': print("listen_tf: Error looking up frame",frame) elif onerror == 'raise': raise return None
def test_getObjectPhalanxMeanContactPoint(self): world = WorldModel() world.loadElement(abspath + '/../data/terrains/plane.env') obj = make_box(world, 0.03, 0.05, 0.1) R, t = obj.getTransform() obj.setTransform(R, [0, 0, 0.1 / 2.]) p = [0.015, -0.025, 0.1] q = [3.7494e-33, 6.12323e-17, 1.0, -6.12323e-17] o_T_p = np.array(se3.homogeneous((so3.from_quaternion(q), p))) w_T_o = np.array(se3.homogeneous(obj.getTransform())) p = (-0.03, -0.12, -0.013 + 0.02 ) # -0.013 + 0.02, from working exp at commit 98305499 R = (0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0) p_T_h = np.array(se3.homogeneous((R, p))) w_T_h_des = w_T_o.dot(o_T_p).dot(p_T_h) w_T_h_des_se3 = se3.from_homogeneous(w_T_h_des) robot = make_moving_base_robot('soft_hand', world) set_moving_base_xform(robot, w_T_h_des_se3[0], w_T_h_des_se3[1]) sim = SimpleSimulator(world) # setup the preshrink visPreshrink = False # turn this to true if you want to see the "shrunken" models used for collision detection for l in range(robot.numLinks()): sim.body(robot.link(l)).setCollisionPreshrink(visPreshrink) for l in range(world.numRigidObjects()): sim.body(world.rigidObject(l)).setCollisionPreshrink(visPreshrink) hand = plugins.soft_hand.HandEmulator(sim, 0, 6, 6) sim.addEmulator(0, hand) links_to_check = np.array([ 3, 4, 6, 8, 10, 13, 15, 17, 20, 22, 24, 27, 29, 31, 33, 35, 37 ]) + 6 #vis.add("world", world) #vis.show() for i in range(100): #vis.lock() hand.setCommand([np.min((i / 10., 1.0))]) sim.simulate(0.01) sim.updateWorld() #vis.unlock() c_p, c_f = getObjectPhalanxMeanContactPoint( sim, obj, robot, links_to_check) c_p_all, c_f_all = getObjectPhalanxMeanContactPoint( sim, obj, robot) if i == 0: self.assertEqual(countContactPoints(c_p), 0) self.assertEqual(countContactPoints(c_p_all), 0) elif i == 99: self.assertTrue(countContactPoints(c_p) > 0) self.assertTrue(countContactPoints(c_p_all) > 0)
def _load_mvbbs(self): try: f = open(self.filename) self.db = {} reader = csv.reader(f) for row in reader: object_dims = tuple( [float(v) for i, v in enumerate(row) if i in range(3)]) t = [float(v) for i, v in enumerate(row) if i in range(3, 6)] q = [float(row[9])] + [ float(v) for i, v in enumerate(row) if i in range(6, 9) ] T = (so3.from_quaternion(q), t) if tuple(object_dims) not in self.db: self.db[object_dims] = [] self.db[object_dims].append(np.array(se3.homogeneous(T))) except: pass
def transform_average(Ts): t = vectorops.div(vectorops.add(*[T[1] for T in Ts]), len(Ts)) qs = [so3.quaternion(T[0]) for T in Ts] q = vectorops.div(vectorops.add(*qs), len(Ts)) return (so3.from_quaternion(q), t)
def from_Quaternion(ros_q): """From ROS Quaternion to Klamp't so3 element""" q = [ros_q.w, ros_q.x, ros_q.y, ros_q.z] return so3.from_quaternion(q)
def so3_staggered_grid(N): """Returns a nx.Graph describing a staggered grid on SO3 with resolution N. The resulting graph has ~8N^3 nodes The node attribute 'params' gives the so3 element. """ import itertools assert N >= 1 G = nx.Graph() R0 = so3.from_quaternion(vectorops.unit((1, 1, 1, 1))) #R0 = so3.identity() namemap = dict() for ax in range(4): for i in xrange(N + 1): u = 2 * float(i) / N - 1.0 u = math.tan(u * math.pi * 0.25) u2 = 2 * float(i + 0.5) / N - 1.0 u2 = math.tan(u2 * math.pi * 0.25) for j in xrange(N + 1): v = 2 * float(j) / N - 1.0 v = math.tan(v * math.pi * 0.25) v2 = 2 * float(j + 0.5) / N - 1.0 v2 = math.tan(v2 * math.pi * 0.25) for k in xrange(N + 1): w = 2 * float(k) / N - 1.0 w = math.tan(w * math.pi * 0.25) w2 = 2 * float(k + 0.5) / N - 1.0 w2 = math.tan(w2 * math.pi * 0.25) idx = [i, j, k] idx = idx[:ax] + [N] + idx[ax:] idx = tuple(idx) if idx in namemap: pass else: namemap[idx] = idx flipidx = tuple([N - x for x in idx]) namemap[flipidx] = idx q = [u, v, w] q = q[:ax] + [1.0] + q[ax:] q = vectorops.unit(q) R = so3.mul(so3.inv(R0), so3.from_quaternion(q)) G.add_node(idx, params=R) if i < N and j < N and k < N: #add staggered point sidx = [i + 0.5, j + 0.5, k + 0.5] sidx = sidx[:ax] + [N] + sidx[ax:] sidx = tuple(sidx) sfidx = tuple([N - x for x in sidx]) namemap[sidx] = sidx namemap[sfidx] = sidx q = [u2, v2, w2] q = q[:ax] + [1.0] + q[ax:] q = vectorops.unit(q) R = so3.mul(so3.inv(R0), so3.from_quaternion(q)) G.add_node(sidx, params=R) for ax in range(4): for i in xrange(N + 1): for j in xrange(N + 1): for k in xrange(N + 1): baseidx = [i, j, k] idx = baseidx[:ax] + [N] + baseidx[ax:] idx = tuple(idx) for n in range(3): nidx = baseidx[:] nidx[n] += 1 if nidx[n] > N: continue nidx = nidx[:ax] + [N] + nidx[ax:] nidx = tuple(nidx) G.add_edge(namemap[idx], namemap[nidx]) #edges between staggered point and grid points baseidx = [i + 0.5, j + 0.5, k + 0.5] if any(v > N for v in baseidx): continue idx = baseidx[:ax] + [N] + baseidx[ax:] idx = tuple(idx) for ofs in itertools.product(*[(-0.5, 0.5)] * 3): nidx = vectorops.add(baseidx, ofs) nidx = [int(x) for x in nidx] nidx = nidx[:ax] + [N] + nidx[ax:] nidx = tuple(nidx) #print "Stagger-grid edge",idx,nidx G.add_edge(namemap[idx], namemap[nidx]) #edges between staggered points -- if it goes beyond face, #go to the adjacent face for n in range(3): nidx = baseidx[:] nidx[n] += 1 if nidx[n] > N: #swap face nidx[n] = N nidx = nidx[:ax] + [N - 0.5] + nidx[ax:] nidx = tuple(nidx) #if not G.has_edge(namemap[idx],namemap[nidx]): # print "Stagger-stagger edge",idx,nidx,"swap face" else: nidx = nidx[:ax] + [N] + nidx[ax:] nidx = tuple(nidx) #if not G.has_edge(namemap[idx],namemap[nidx]): # print "Stagger-stagger edge",idx,nidx G.add_edge(namemap[idx], namemap[nidx]) return G
def test_rpy(self): R = so3.from_quaternion((-4.32978e-17, -0.707107,4.32978e-17,0.707107)) r,p,y = so3.rpy(R) self.assertAlmostEqual(r,0.0) self.assertAlmostEqual(p,1.5707963267948966) self.assertAlmostEqual(y,3.141592653589793)
def normalize_rotation(R): """Given a matrix close to a proper (orthogonal) rotation matrix, returns a true orthogonal matrix.""" q = so3.quaternion(R) #normalizes return so3.from_quaternion(q)
import threading from sspp.service import Service from sspp.topic import TopicListener import asyncore # parameters for initial robot pose q_init_left = [-0.08897088559570313, -0.9675583808532715, -1.2034079267211915, 1.7132575355041506, 0.6776360122741699, 1.0166457660095216, -0.5065971546203614] q_init_right = [0.08897088559570313, -0.9675583808532715, 1.2034079267211915, 1.7132575355041506, -0.6776360122741699, 1.0166457660095216, 0.5065971546203614] q_tuckarm_left = [0.58897088559570313, -0.9675583808532715, -1.2034079267211915, 1.7132575355041506, 0.6776360122741699, 1.0166457660095216, -0.5065971546203614] q_tuckarm_right = [-0.58897088559570313, -0.9675583808532715, 1.2034079267211915, 1.7132575355041506, -0.6776360122741699, 1.0166457660095216, 0.5065971546203614] q = (0.564775,0.425383,0.426796,0.563848) R1 = so3.from_quaternion(q) t1 = (0.227677,0.0916972,0.0974174) kinect_frame_to_pedestal = (R1, t1) #Configuration variable: where's the state server? system_state_addr = EbolabotSystemConfig.getdefault_ip('state_server_computer_ip',('localhost',4568)) #imaging stuff try: from PIL import Image except ImportError, err: import Image class GLTexture:
B = so3.from_quaternion(A) # [a11, a21, a31, ...] C = so3.matrix(B) T = np.eye(4) T[:3, :3] = C T[:3, 3] = t out_data.append(list(T[0]) + list(T[1]) + list(T[2]) + list(T[3])) """ TT = [0.492127,0.003738762,0.08270439,-0.02360799,0.7022632,-0.3517123,-0.5801101,-0.2158636] # from advio-04 TT = [0.514813,-0.005759389,-0.09049783,-0.001374606,0.64807,-0.264358,0.6264483,0.343049] # a complete line from advio-23 A = TT[4: 8] B = so3.from_quaternion(A) print(B) C = so3.matrix(B) print(C) t = TT[1: 4] T = np.eye(4) T[:3, :3] = C T[:3, 3] = t print(T) print([list(T[0]), list(T[1]), list(T[2]), list(T[3])]) print(np.array([list(T[0]), list(T[1]), list(T[2]), list(T[3])])) print(list(T[0]) + list(T[1]) + list(T[2]) + list(T[3]))