def launch_test_mvbb_grasps(robotname, box_db, links_to_check = None): """Launches a very simple program that spawns a box with dimensions specified in boxes_db. """ world = WorldModel() world.loadElement("data/terrains/plane.env") robot = make_moving_base_robot(robotname, world) xform = resource.get("default_initial_%s.xform" % robotname, description="Initial hand transform", default=se3.identity(), world=world, doedit=False) for box_dims, poses in box_db.db.items(): if world.numRigidObjects() > 0: world.remove(world.rigidObject(0)) obj = make_box(world, box_dims[0], box_dims[1], box_dims[2]) poses_filtered = [] R,t = obj.getTransform() obj.setTransform(R, [0, 0, box_dims[2]/2.]) w_T_o = np.array(se3.homogeneous(obj.getTransform())) p_T_h = np.array(se3.homogeneous(xform)) p_T_h[2][3] += 0.02 for pose in poses: w_T_p = w_T_o.dot(pose) w_T_h_des_se3 = se3.from_homogeneous(w_T_p.dot(p_T_h)) if CollisionTestPose(world, robot, obj, w_T_h_des_se3): pose_pp = se3.from_homogeneous(pose) t_pp = pose_pp[1] q_pp = so3.quaternion(pose_pp[0]) q_pp = [q_pp[1], q_pp[2], q_pp[3], q_pp[0]] print "Pose", t_pp + q_pp, "has been filtered since it is in collision for box", box_dims elif w_T_p[2][3] <= 0.: print "Pose", t_pp + q_pp, "has been filtered since it penetrates with table" else: poses_filtered.append(pose) print "Filtered", len(poses)-len(poses_filtered), "out of", len(poses), "poses" # embed() # create a hand emulator from the given robot name module = importlib.import_module('plugins.' + robotname) # emulator takes the robot index (0), start link index (6), and start driver index (6) program = FilteredMVBBTesterVisualizer(box_dims, poses_filtered, world, p_T_h, module, box_db, links_to_check) vis.setPlugin(None) vis.setPlugin(program) program.reshape(800, 600) vis.show() # this code manually updates the visualization while vis.shown(): time.sleep(0.1) return
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_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 display(self): if self.running: self.world.drawGL() w_T_o = np.array(se3.homogeneous(self.obj.getTransform())) for pose in self.poses: w_T_p_des = w_T_o.dot(pose) w_T_p__des_se3 = se3.from_homogeneous(w_T_p_des) draw_GL_frame(w_T_p__des_se3, color=(0.5,0.5,0.5)) if self.curr_pose is not None: w_T_p_des = w_T_o.dot(self.curr_pose) w_T_p__des_se3 = se3.from_homogeneous(w_T_p_des) draw_GL_frame(w_T_p__des_se3) hand_xform = get_moving_base_xform(self.robot) w_T_p = np.array(se3.homogeneous(hand_xform)).dot(self.h_T_p) w_T_p_se3 = se3.from_homogeneous(w_T_p) draw_GL_frame(w_T_p_se3) glDisable(GL_LIGHTING) glDisable(GL_DEPTH_TEST) glEnable(GL_POINT_SMOOTH) glColor3f(1,1,0) glLineWidth(1.0) glPointSize(5.0) forceLen = 0.01 # scale of forces if self.sim is not None and self.obj is not None and self.robot is not None: c_p, c_f = getObjectPhalanxMeanContactPoint(self.sim, self.obj, self.robot, self.links_to_check) n_c_p = countContactPoints(c_p) if countContactPoints(c_p) > 0: glBegin(GL_POINTS) for i in range(len(c_p)/3): o_c_p_i = c_p[3*i:3*i+3] if np.all([False if math.isnan(p) else True for p in o_c_p_i]): w_c_p_i = se3.apply(se3.from_homogeneous(w_T_o), o_c_p_i) glVertex3f(*w_c_p_i) glEnd() glBegin(GL_LINES) for i in range(len(c_p)/3): o_c_p_i = c_p[3 * i:3 * i + 3] o_c_f_i = c_f[6 * i:6 * i + 3] if np.all([False if math.isnan(f) else True for f in o_c_f_i]): if np.all([False if math.isnan(p) else True for p in o_c_p_i]): w_c_p_i = se3.apply(se3.from_homogeneous(w_T_o), o_c_p_i) w_c_f_i = se3.apply(se3.from_homogeneous(w_T_o), o_c_f_i) glVertex3f(*w_c_p_i) glVertex3f(*vectorops.madd(w_c_p_i, w_c_f_i, forceLen)) glEnd()
def setGhostConfig(self,q,name="ghost",robot=0): """Sets the configuration of the ghost to q. If the ghost is named, place its name in prefixname.""" if robot < 0 or robot >= self.world.numRobots(): raise ValueError("Invalid robot specified") robot = self.world.robot(robot) q_original = robot.getConfig() if len(q) != robot.numLinks(): raise ValueError("Config must be correct size: %d != %d"%(len(q),robot.numLinks())) robot.setConfig(q) self.beginRpc(strict=False) rpcs = [] for i in range(robot.numLinks()): T = robot.link(i).getTransform() p = robot.link(i).getParent() if p>=0: Tp = robot.link(p).getTransform() T = se3.mul(se3.inv(Tp),T) mat = se3.homogeneous(T) #mat is now a 4x4 homogeneous matrix linkname = name+robot.link(i).getName() #send to the ghost link with name "name"... self._do_rpc({'type':'set_transform','object':linkname,'matrix':[mat[0][0],mat[0][1],mat[0][2],mat[0][3],mat[1][0],mat[1][1],mat[1][2],mat[1][3],mat[2][0],mat[2][1],mat[2][2],mat[2][3],mat[3][0],mat[3][1],mat[3][2],mat[3][3]]}) self.endRpc(strict=False) robot.setConfig(q_original) #restore original config
def draw_wand(self,xform,size = 0.2): #draw the wand pointing along the -x axis (+x toward the user) gldraw.setcolor(1,0.8,0.8) mat = zip(*se3.homogeneous(xform)) mat = sum([list(coli) for coli in mat],[]) glPushMatrix() glMultMatrixf(mat) gldraw.box([-size,-size*0.05,-size*0.05],[size*0.5,size*0.05,size*0.05]) glPopMatrix()
def has_simulation(self, box_dims, pose): self._load_mvbbs_simulated() box_dims = tuple(box_dims) if box_dims in self.db_simulated: if not isinstance(pose, np.ndarray): pose = np.array(se3.homogeneous(pose)) poses = [p['T'] for p in self.db_simulated[box_dims]] for p in poses: if np.all((pose - p) < 1e-12): return True return False
def redraw_pcd(pcd_pair, Toct_needle): """ :param pcd_pair: {list} -- The list of Open3D.PointCloud :param Toct_needle: {list} -- The transfer matrix from OCT frame to needle frame :return: None """ source = pcd_pair[0] target = pcd_pair[1] source_temp = copy.deepcopy(source) target_temp = copy.deepcopy(target) source_temp.paint_uniform_color([1, 0.706, 0]) target_temp.paint_uniform_color([0, 0.651, 0.929]) target_temp.transform(se3.homogeneous(Toct_needle)) return [source_temp, target_temp]
def setGhostConfig(self, q, name="ghost", robot=0): """Sets the configuration of the ghost to q. If the ghost is named, place its name in prefixname.""" if robot < 0 or robot >= self.world.numRobots(): raise ValueError("Invalid robot specified") robot = self.world.robot(robot) q_original = robot.getConfig() if len(q) != robot.numLinks(): raise ValueError("Config must be correct size: %d != %d" % (len(q), robot.numLinks())) robot.setConfig(q) self.beginRpc(strict=False) rpcs = [] for i in range(robot.numLinks()): T = robot.link(i).getTransform() p = robot.link(i).getParent() if p >= 0: Tp = robot.link(p).getTransform() T = se3.mul(se3.inv(Tp), T) mat = se3.homogeneous(T) #mat is now a 4x4 homogeneous matrix linkname = name + robot.link(i).getName() #send to the ghost link with name "name"... self._do_rpc({ 'type': 'set_transform', 'object': linkname, 'matrix': [ mat[0][0], mat[0][1], mat[0][2], mat[0][3], mat[1][0], mat[1][1], mat[1][2], mat[1][3], mat[2][0], mat[2][1], mat[2][2], mat[2][3], mat[3][0], mat[3][1], mat[3][2], mat[3][3] ] }) self.endRpc(strict=False) robot.setConfig(q_original) #restore original config
def __init__(self, box_dims, poses, world, p_T_h, module, box_db, links_to_check=None): GLRealtimeProgram.__init__(self, "FilteredMVBBTEsterVisualizer") self.world = world self.p_T_h = p_T_h self.h_T_p = np.linalg.inv(self.p_T_h) self.poses = [] self.hand = None self.is_simulating = False self.curr_pose = None self.robot = self.world.robot(0) self.q_0 = self.robot.getConfig() self.w_T_o = None self.obj = None self.box_dims = box_dims self.t_0 = None self.object_com_z_0 = None self.object_fell = None self.sim = None self.module = module self.running = True self.db = box_db self.links_to_check = links_to_check if self.world.numRigidObjects() > 0: self.obj = self.world.rigidObject(0) self.w_T_o = np.array(se3.homogeneous(self.obj.getTransform())) for p in poses: if not self.db.has_simulation(self.box_dims, p): self.poses.append(p) else: print "Pose", p, "already simulated" else: "Warning: during initialization of visualizer object is still not loaded in world" self.poses = poses print "Will simulate", len(self.poses), "poses,"
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 trans_all2needle(Tlink_set, Toct, Tneedle, pcd_list): """ :param Tlink_set: {list} -- the robot configuration list :param Toct: {tuple} -- the transformation matrix from world to OCT :param Tneedle: {tuple} -- the transformation matrix from robot end effector to needle :param pcd_list: {list} -- the point cloud list :return: None """ pcd_needle_list = [] pcd_t = copy.deepcopy(pcd_list[0][1]) pcd_t.paint_uniform_color([0, 0.651, 0.929]) pcd_needle_list.append(pcd_t) mesh_frame = create_mesh_coordinate_frame(0.0006, [0, 0, 0]) pcd_needle_list.append(mesh_frame) for i in range(len(Tlink_set)): Tneedle2oct = se3.mul(se3.inv(Tneedle), se3.mul(se3.inv(Tlink_set[i]), Toct)) pcd_s_copy = copy.deepcopy(pcd_list[i][0]) pcd_s_copy.transform(se3.homogeneous(Tneedle2oct)) pcd_s_copy.paint_uniform_color([0.4 + i * 0.1, 0.706, 0]) pcd_needle_list.append(pcd_s_copy) draw_geometries(pcd_needle_list)
def idle(self): if not self.running: return if self.world.numRigidObjects() > 0: self.obj = self.world.rigidObject(0) else: return if not self.is_simulating: if len(self.poses) > 0: self.curr_pose = self.poses.pop(0) print "\n\n\n!!!!!!!!!!!!!!!!!!!!!!!!!!" print "!!!!!!!!!!!!!!!!!!!!!!!!!!" print "!!!!!!!!!!!!!!!!!!!!!!!!!!" print "Simulating Next Pose Grasp" print "Dims:\n", self.box_dims print "Pose:\n", self.curr_pose print "!!!!!!!!!!!!!!!!!!!!!!!!!!" print "!!!!!!!!!!!!!!!!!!!!!!!!!!" print "!!!!!!!!!!!!!!!!!!!!!!!!!!\n\n\n" else: print "Done testing all", len( self.poses), "poses for object", self.box_dims print "Quitting" self.running = False vis.show(hidden=True) return w_T_o_se3 = se3.from_homogeneous(self.w_T_o) self.obj.setTransform(w_T_o_se3[0], w_T_o_se3[1]) w_T_h_des_se3 = se3.from_homogeneous( self.w_T_o.dot(self.curr_pose).dot(self.p_T_h)) self.robot.setConfig(self.q_0) set_moving_base_xform(self.robot, w_T_h_des_se3[0], w_T_h_des_se3[1]) if self.sim is None: self.sim = SimpleSimulator(self.world) self.hand = self.module.HandEmulator(self.sim, 0, 6, 6) self.sim.addEmulator(0, self.hand) # the next line latches the current configuration in the PID controller... self.sim.controller(0).setPIDCommand( self.robot.getConfig(), vectorops.mul(self.robot.getVelocity(), 0.0)) obj_b = self.sim.body(self.obj) obj_b.setVelocity([0., 0., 0.], [0., 0., 0.]) # 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(self.robot.numLinks()): self.sim.body( self.robot.link(l)).setCollisionPreshrink(visPreshrink) for l in range(self.world.numRigidObjects()): self.sim.body(self.world.rigidObject( l)).setCollisionPreshrink(visPreshrink) self.object_com_z_0 = getObjectGlobalCom(self.obj)[2] self.object_fell = False self.t_0 = self.sim.getTime() self.is_simulating = True if self.is_simulating: t_lift = 1.3 # when to lift d_lift = 1.0 # duration # print "t:", self.sim.getTime() - self.t_0 object_com_z = getObjectGlobalCom(self.obj)[2] w_T_h_curr_se3 = get_moving_base_xform(self.robot) w_T_h_des_se3 = se3.from_homogeneous( self.w_T_o.dot(self.curr_pose).dot(self.p_T_h)) if self.sim.getTime() - self.t_0 == 0: # print "Closing hand" self.hand.setCommand([1.0]) elif (self.sim.getTime() - self.t_0) >= t_lift and ( self.sim.getTime() - self.t_0) <= t_lift + d_lift: # print "Lifting" t_i = w_T_h_des_se3[1] t_f = vectorops.add(t_i, (0, 0, 0.2)) u = np.min((self.sim.getTime() - self.t_0 - t_lift, 1.)) send_moving_base_xform_PID(self.sim.controller(0), w_T_h_des_se3[0], vectorops.interpolate(t_i, t_f, u)) if (self.sim.getTime() - self.t_0 ) >= t_lift: # wait for a lift before checking if object fell d_hand = w_T_h_curr_se3[1][2] - w_T_h_des_se3[1][2] d_com = object_com_z - self.object_com_z_0 if d_hand - d_com > 0.1: self.object_fell = True print "!!!!!!!!!!!!!!!!!!" print "Grasp Unsuccessful" print "!!!!!!!!!!!!!!!!!!" self.sim.simulate(0.01) self.sim.updateWorld() if not vis.shown() or (self.sim.getTime() - self.t_0) >= 2.5 or self.object_fell: if vis.shown(): # simulation stopped because it was successful print "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%" print "Saving grasp, status:", "failure" if self.object_fell else "success" print "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\n\n" w_T_h_curr = np.array(se3.homogeneous(w_T_h_curr_se3)) w_T_o_curr = np.array( se3.homogeneous(self.obj.getTransform())) h_T_o = np.linalg.inv(w_T_h_curr).dot(w_T_o_curr) if self.db.n_dofs == self.hand.d_dofs + self.hand.u_dofs: q_grasp = [ float('nan') ] * self.db.n_dofs if self.object_fell else self.hand.getConfiguration( ) elif self.db.n_dofs == self.hand.d_dofs + self.hand.u_dofs + self.hand.m_dofs: q_grasp = [ float('nan') ] * self.db.n_dofs if self.object_fell else self.hand.getFullConfiguration( ) else: raise Exception( 'Error: unexcpeted number of joints for hand') c_p, c_f = getObjectPhalanxMeanContactPoint( self.sim, self.obj, self.robot, self.links_to_check) try: self.db.save_simulation(self.box_dims, self.curr_pose, h_T_o, q_grasp, c_p, c_f) except: print "\nXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX" print "XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX" print "X Error while calling save_simulation X" print "XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX" traceback.print_exc() print "XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX\n" self.is_simulating = False self.sim = None self.robot.setConfig(self.q_0)
def to_numpy(obj, type='auto'): """Converts a Klamp't object to a numpy array or multiple numpy arrays. Supports: * lists and tuples * RigidTransform: returned as 4x4 homogeneous coordinate transform * Matrix3, Rotation: returned as 3x3 matrix. Can't be determined with 'auto', need to specify type='Matrix3' or 'Rotation'. * Configs * Trajectory: returns a pair (times,milestones) * TriangleMesh: returns a pair (verts,indices) * PointCloud: returns a n x (3+k) array, where k is the # of properties * VolumeGrid: returns a triple (bmin,bmax,array) * Geometry3D: returns a pair (T,geomdata) If you want to get a transformed point cloud or mesh, you can pass in a Geometry3D as the obj, and its geometry data type as the type. """ global supportedTypes if type == 'auto': otype = types.objectToTypes(obj) if isinstance(otype, (list, tuple)): for t in otype: if t in supportedTypes: type = t break if type == 'auto': raise ValueError('obj is not a supported type: ' + ', '.join(otype)) else: type = otype if type not in supportedTypes: raise ValueError(type + ' is not a supported type') if type == 'RigidTransform': return np.array(se3.homogeneous(obj)) elif type == 'Rotation' or type == 'Matrix3': return np.array(so3.matrix(obj)) elif type == 'Trajectory': return np.array(obj.times), np.array(obj.milestones) elif type == 'TriangleMesh': from klampt import Geometry3D if isinstance(obj, Geometry3D): res = to_numpy(obj.getTriangleMesh(), type) R = to_numpy(obj.getCurrentTransform()[0], 'Matrix3') t = to_numpy(obj.getCurrentTransform()[1], 'Vector3') return (np.dot(R, res[0]) + t, res[1]) return (np.array(obj.vertices).reshape((len(obj.vertices) // 3, 3)), np.array(obj.indices, dtype=np.int32).reshape( (len(obj.indices) // 3, 3))) elif type == 'PointCloud': from klampt import Geometry3D if isinstance(obj, Geometry3D): res = to_numpy(obj.getPointCloud(), type) R = to_numpy(obj.getCurrentTransform()[0], 'Matrix3') t = to_numpy(obj.getCurrentTransform()[1], 'Vector3') res[:, :3] = np.dot(R, res[:, :3]) + t return res points = np.array(obj.vertices).reshape((obj.numPoints(), 3)) if obj.numProperties() == 0: return points properties = np.array(obj.properties).reshape( (obj.numPoints(), obj.numProperties())) return np.hstack((points, properties)) elif type == 'VolumeGrid': bmin = np.array(obj.bbox)[:3] bmax = np.array(obj.bbox)[3:] values = np.array(obj.values).reshape( (obj.dims[0], obj.dims[1], obj.dims[2])) return (bmin, bmax, values) elif type == 'Geometry3D': if obj.type() == 'PointCloud': return to_numpy(obj.getCurrentTransform(), 'RigidTransform'), to_numpy( obj.getPointCloud(), obj.type()) elif obj.type() == 'TriangleMesh': return to_numpy(obj.getCurrentTransform(), 'RigidTransform'), to_numpy( obj.getTriangleMesh(), obj.type()) elif obj.type() == 'VolumeGrid': return to_numpy(obj.getCurrentTransform(), 'RigidTransform'), to_numpy( obj.getVolumeGrid(), obj.type()) elif obj.type() == 'Group': arrays = [] for i in range(obj.numElements()): arrays.append(to_numpy(obj.getElement(i), 'Geometry3D')) return to_numpy(obj.getCurrentTransform(), 'RigidTransform'), arrays else: return np.array(obj)
def optimization(self): """ :return: None """ time_stamp = time.time() min_error = 10e5 for k in range(25): random.seed(time.time()) est_input = [random.uniform(-2 * np.pi, 2 * np.pi) for i in range(3)] + \ [random.uniform(-0.8, 0.8) for i in range(3)] + \ [random.uniform(-2 * np.pi, 2 * np.pi) for i in range(3)] + \ [random.uniform(-0.2, 0.2) for i in range(3)] print("We will start " + str(k + 1) + "th minimize!") res = root(opt_error_fun, np.array(est_input), args=(self.Tlink_set, self.trans_list), method='lm', options={}) print("The reason for termination: \n", res.message, "\nAnd the number of the iterations are: ", res.nfev) error = np.sum(np.absolute(res.fun)) print("The minimize is end, and the error is: ", error) if error <= min_error: min_error = error self.min_res = res print("The optimization uses: ", time.time() - time_stamp, "seconds") print("The error is: ", np.sum(np.absolute(self.min_res.fun))) print("The optimized T_oct is: ", (self.min_res.x[0:3], self.min_res.x[3:6])) print( "And the matrix form is: \n", np.array( se3.homogeneous( (so3.from_rpy(self.min_res.x[0:3]), self.min_res.x[3:6])))) print("The optimized T_needle_end is: ", (self.min_res.x[6:9], self.min_res.x[9:12])) print( "And the matrix form is: \n", np.array( se3.homogeneous((so3.from_rpy(self.min_res.x[6:9]), self.min_res.x[9:12])))) # os.makedirs("../../data/suture_experiment/calibration_result_files/" + self.serial_num + "/", exist_ok=True) # np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num + # "/calibration_result.npy", self.min_res.x, allow_pickle=True) vis.spin(float('inf')) vis.clear() self.robot.setConfig(self.qstart) vis.add("world", self.world) World_base = model.coordinates.Frame("Base Frame", worldCoordinates=(so3.from_rpy( [0, 0, 0]), [0, 0, 0])) vis.add('World_base', World_base) est_OCT_coordinate = model.coordinates.Frame( "est OCT Frame", worldCoordinates=(so3.from_rpy(self.min_res.x[0:3]), self.min_res.x[3:6])) vis.add("est OCT Frame", est_OCT_coordinate) Tlink_6 = model.coordinates.Frame( "Link 6 Frame", worldCoordinates=self.robot.link('link_6').getTransform()) vis.add('Link 6 Frame', Tlink_6) est_Tneedle_world = se3.mul( self.robot.link('link_6').getTransform(), (so3.from_rpy(self.min_res.x[6:9]), self.min_res.x[9:12])) est_needle_coordinate = model.coordinates.Frame( "est needle Frame", worldCoordinates=est_Tneedle_world) vis.add("est needle Frame", est_needle_coordinate) vis.spin(float('inf'))
def needle2oct_icp(self, thres_t, thres_s): """ :return: None """ threshold = 50 for i in range(0, self.num_pcd): pcd_s = read_point_cloud( "../../data/point_clouds/source_point_clouds/" + self.serial_num + "/cropped_OCT_config_" + str(i + 1) + ".ply") pcd_s.points = Vector3dVector( np.multiply(np.asarray(pcd_s.points), 1e-3)) if i < thres_t: pcd_t = read_point_cloud( "../../data/point_clouds/target_point_clouds/TF-1-cut.ply") else: pcd_t = read_point_cloud( "../../data/point_clouds/target_point_clouds/TF-1.ply") pcd_t.points = Vector3dVector( np.multiply(np.asarray(pcd_t.points), 1e-3)) temp_rmse = 1. count = 0 termination_threshold = 2.0e-5 while temp_rmse > termination_threshold: print("ICP implementation is in " + str(i + 1) + "th point clouds.") random.seed(count * 5 + 7 - count / 3) if i > thres_s: trans_init = np.asarray( se3.homogeneous((so3.from_rpy([ random.uniform(-0.5 * np.pi, 0.5 * np.pi) for kk in range(3) ]), [ random.uniform(-5. * 1e-3, 5. * 1e-3) for kk in range(3) ]))) else: trans_init = np.asarray( se3.homogeneous((so3.from_rpy([ random.uniform(-0.5 * np.pi, 0.5 * np.pi) + np.pi ] + [ random.uniform(-0.5 * np.pi, 0.5 * np.pi) for kk in range(2) ]), [random.uniform(-5. * 1e-3, 5. * 1e-3)] + [ random.uniform(-5. * 1e-3, 5. * 1e-3) + 15. * 1e-3 ] + [random.uniform(-5. * 1e-3, 5. * 1e-3)]))) reg_p2p = registration_icp( pcd_s, pcd_t, threshold, trans_init, TransformationEstimationPointToPoint(), ICPConvergenceCriteria(max_iteration=300, relative_rmse=1e-15, relative_fitness=1e-15)) # ***DEBUG*** # # draw_registration_result(pcd_s, pcd_t, reg_p2p.transformation) modified_pcd_s = duplicate_part_pcd(reg_p2p.transformation, pcd_s, -0.2 * 1e-3, 0.0 * 1e-3, 30) reg_p2p_2 = registration_icp( modified_pcd_s, pcd_t, threshold, reg_p2p.transformation, TransformationEstimationPointToPoint(), ICPConvergenceCriteria(max_iteration=100, relative_rmse=1e-15, relative_fitness=1e-15)) # ***DEBUG*** # # draw_registration_result(modified_pcd_s, pcd_t, reg_p2p_2.transformation) print("The RMSE is: ", reg_p2p_2.inlier_rmse) temp_rmse = reg_p2p_2.inlier_rmse count += 1 if count == 15: termination_threshold = 2.5e-5 trans = se3.from_homogeneous(reg_p2p_2.transformation) self.trans_list.append(trans) point_oct = [] pcd_s_copy = PointCloud() pcd_p = PointCloud() pcd_s_copy.points = Vector3dVector( np.multiply(np.asarray(modified_pcd_s.points), 1)) pcd_t_copy = read_point_cloud( "../../data/point_clouds/target_point_clouds/TF-1.ply") pcd_t_copy.points = Vector3dVector( np.multiply(np.asarray(pcd_t_copy.points), 1e-3)) pcd_p.points = Vector3dVector(point_oct) # ***DEBUG*** # # draw_registration_result(pcd_s_copy, pcd_t_copy, se3.homogeneous(trans)) self.pcd_list.append([pcd_s_copy, pcd_t_copy])