def compute_tissue_pose(self, nw, ne, sw): x = -0.01 y = 0.1 z = 0.01 roll = 1.5 # in degrees yaw = 2 pitch = 3.5 nw_position = np.hstack(np.array(nw.position)) ne_position = np.hstack(np.array(ne.position)) sw_position = np.hstack(np.array(sw.position)) u = sw_position - nw_position v = ne_position - nw_position self.tissue_length = norm(u) self.tissue_width = norm(v) u /= self.tissue_length v /= self.tissue_width origin = nw_position # origin of palpation board is the NW corner w = np.cross(u, v) u = np.cross(v, w) # to ensure that all axes are orthogonal rotation_matrix = np.array([u,v,w]).transpose() # offset = np.dot(rotation_matrix, np.array([-0.009, 0.100, 0.01])) pose = tfx.pose(origin, rotation_matrix, frame=nw.frame) pose = pose.as_tf()*tfx.transform([x,y,z])*tfx.transform(tfx.rotation_tb(0, 0, roll))*tfx.transform(tfx.rotation_tb(0, pitch, 0))*tfx.transform(tfx.rotation_tb(yaw, 0, 0)) self.tissue_pose = pose self.tissue_width = 0.05 self.tissue_length = 0.025 return pose
def broadcast(self): f = open( "/home/davinci2/catkin_ws/src/davinci_vision/launch/BC_registration/camera_frame.p", "rb") self.camera_transform = pickle.load(f) f.close() #SOMETIMES NEED TO INVERT X & Y AXES; just change from 1 to -1 and vice versa offset = tfx.transform([SKETCH_OFFSET, 0, CAP_OFFSET], [[1, 0, 0], [0, 1, 0], [0, 0, 1]]) self.camera_transform = tfx.transform( self.camera_transform).as_transform() * offset transform = tfx.inverse_tf(self.camera_transform) pt = transform.translation rot = transform.rotation msg = TransformStamped() msg.header.stamp = rospy.Time.now() msg.transform.rotation.x = rot.x msg.transform.rotation.y = rot.y msg.transform.rotation.z = rot.z msg.transform.rotation.w = rot.w msg.child_frame_id = "left_BC" msg.transform.translation.x = pt.x msg.transform.translation.y = pt.y msg.transform.translation.z = pt.z msg.header.frame_id = "registration_brick_right" msg.header.stamp = rospy.Time.now() print('boo') while not rospy.is_shutdown(): msg.header.stamp = rospy.Time.now() self.pub.publish([msg]) rospy.sleep(0.1)
def gripper_pose(self, R_gripper_center = np.eye(3), t_gripper_center = PR2_GRASP_OFFSET): """ Convert a grasp to a gripper pose in SE(3) using PR2 gripper_l_tool_frame as default Params: R_gripper_center: (numpy 3x3 array) rotation matrix from grasp basis to gripper basis t_gripper_center: (numpy 3 array) translation from grasp basis to gripper basis Returns: pose_gripper: (tfx transform) pose of gripper in the grasp frame """ # convert gripper orientation to rotation matrix grasp_axis_y = self.axis_ grasp_axis_x = np.array([grasp_axis_y[1], -grasp_axis_y[0], 0]) grasp_axis_x = grasp_axis_x / np.linalg.norm(grasp_axis_x) grasp_axis_z = np.cross(grasp_axis_x, grasp_axis_y) R_center_ref = np.c_[grasp_axis_x, np.c_[grasp_axis_y, grasp_axis_z]] pose_center_ref = tfx.transform(R_center_ref, self.center_) # pose of grasp center in its reference frame # rotate along grasp approach angle R_center_rot_center = np.array([[ np.cos(self.approach_angle_), 0, np.sin(self.approach_angle_)], [ 0, 1, 0], [-np.sin(self.approach_angle_), 0, np.cos(self.approach_angle_)]]) pose_center_rot_center = tfx.transform(R_center_rot_center, np.zeros(3)) pose_gripper_center_rot = tfx.transform(R_gripper_center, t_gripper_center) pose_gripper_ref = pose_center_ref.apply(pose_center_rot_center).apply(pose_gripper_center_rot) return pose_gripper_ref """
def broadcast(self): f = open("/home/davinci2/catkin_ws/src/davinci_vision/launch/BC_registration/camera_frame.p", "rb") self.camera_transform = pickle.load(f) f.close() #SOMETIMES NEED TO INVERT X & Y AXES; just change from 1 to -1 and vice versa offset = tfx.transform([SKETCH_OFFSET, 0, CAP_OFFSET], [[1, 0, 0], [0, 1, 0], [0, 0, 1]]) self.camera_transform = tfx.transform(self.camera_transform).as_transform() * offset transform = tfx.inverse_tf(self.camera_transform) pt = transform.translation rot = transform.rotation msg = TransformStamped() msg.header.stamp = rospy.Time.now() msg.transform.rotation.x = rot.x msg.transform.rotation.y = rot.y msg.transform.rotation.z = rot.z msg.transform.rotation.w = rot.w msg.child_frame_id = "left_BC" msg.transform.translation.x = pt.x msg.transform.translation.y = pt.y msg.transform.translation.z = pt.z msg.header.frame_id = "registration_brick_right" msg.header.stamp = rospy.Time.now() print('boo') while not rospy.is_shutdown(): msg.header.stamp = rospy.Time.now() self.pub.publish([msg]) rospy.sleep(0.1)
def load_tissue_pose_from_registration_brick_pose(self): # tissue pose offsets: x = -0.048 y = 0.07 z = 0.006 rotation = 0.0 tissue_frame = pickle.load(open("registration_brick_pose.p", "rb")) tissue_frame = tissue_frame.as_tf()*tfx.transform([x,y,z])*tfx.transform(tfx.rotation_tb(rotation, 0, 0)) self.tissue_pose = tfx.pose(tissue_frame) self.tissue_width = 0.056 self.tissue_length = 0.028
def __init__(self, arms = ['L','R'], calcPosePostAdjustment=None, adjustmentInterpolation=True, errorCorrectionMode='NONE'): self.arms = arms self.errorCorrectionMode = errorCorrectionMode self.truthPose = {} self.calcPose = {} self.calcPoseAtTruth = {} self.sys_error = {} self.sys_error= {} self.est_pose_pub = {} self.pose_error_pub = {} self.estimateFromImage = dict() self.estimatedPose = defaultdict(lambda: (None,None)) if self.errorCorrectionMode == 'SYS': trained_data_R = pickle.load(open(os.path.join(roslib.packages.get_pkg_subdir('raven_2_params','data/right_arm'),'right_train_data.pkl'))) trained_data_L = pickle.load(open(os.path.join(roslib.packages.get_pkg_subdir('raven_2_params','data/left_arm'),'left_train_data.pkl'))) self.sys_error['R'] = tfx.transform(trained_data_R["sys_robot_tf"]['R']) self.sys_error['L'] = tfx.transform(trained_data_L["sys_robot_tf"]['L']) print self.sys_error['R'] print self.sys_error['L'] elif self.errorCorrectionMode == 'PRE_POST': self.pre_adjustment = dict((arm,tfx.identity_tf()) for arm in self.arms) self.post_adjustment = dict((arm,tfx.identity_tf()) for arm in self.arms) self.adjustment_side = dict((arm,'post') for arm in self.arms) self.switch_adjustment_update = True self.adjustmentInterpolation = {'pre': 1, 'post': 1} if adjustmentInterpolation is True: self.adjustmentInterpolation['pre'] = self.adjustmentInterpolation['post'] = 0.5 elif isinstance(adjustmentInterpolation,dict): self.adjustmentInterpolation['pre'] = adjustmentInterpolation.get('pre',0.5) self.adjustmentInterpolation['post'] = adjustmentInterpolation.get('post',0.5) elif isinstance(adjustmentInterpolation,(list,tuple)): self.adjustmentInterpolation['pre'] = adjustmentInterpolation[0] self.adjustmentInterpolation['post'] = adjustmentInterpolation[1] else: self.adjustmentInterpolation['pre'] = self.adjustmentInterpolation['post'] = adjustmentInterpolation self.calcPosePostAdjustment = dict((arm,tfx.identity_tf()) for arm in self.arms) if calcPosePostAdjustment: for k,v in calcPosePostAdjustment: self.calcPosePostAdjustment[k] = tfx.transform(v,copy=True) self.pre_adj_pub = {} self.post_adj_pub = {} for arm in self.arms: rospy.Subscriber(raven_constants.GripperTape.Topic+'_'+arm, PoseStamped, partial(self._truthCallback,arm)) self.pre_adj_pub[arm] = rospy.Publisher('estimated_gripper_pose_pre_adjument_%s'%arm, TransformStamped) self.post_adj_pub[arm] = rospy.Publisher('estimated_gripper_pose_post_adjument_%s'%arm, TransformStamped) for arm in self.arms: self.est_pose_pub[arm] = rospy.Publisher('estimated_gripper_pose_%s'%arm, PoseStamped) self.pose_error_pub[arm] = rospy.Publisher('estimated_gripper_pose_error_%s'%arm, PoseStamped) rospy.Subscriber(raven_constants.RavenTopics.RavenState, RavenState, self._ravenStateCallback)
def __init__(self, arm, sim, tool_to_camera=None, height=480., width=640., focal_length=.01, fx=525., fy=525., cx=319.5, cy=239.5, min_range=.3, max_range=1.5): #fx=640.*2., fy=480.*2., cx=640./2.+0.5, cy=480./2.+0.5, max_range=1.5): self.arm = arm self.sim = sim self.tool_to_camera = tool_to_camera if tool_to_camera is not None else tfx.transform( wrist_to_hand) self.height = height self.width = width self.fx, self.fy, self.cx, self.cy = fx, fy, cx, cy self.P = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) self.focal_length = focal_length # TODO: not sure if needed self.min_range = min_range self.max_range = max_range self.height_m = focal_length * (height / fy) self.width_m = focal_length * (width / fx)
def test_2d_transform(): sdf_2d_file_name = 'data/test/sdf/medium_black_spring_clamp_optimized_poisson_texture_mapped_mesh_clean_0.csv' sf2 = sf.SdfFile(sdf_2d_file_name) sdf_2d = sf2.read() # transform pose_mat = np.matrix([[0, 1, 0, 0], [-1, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) tf = tfx.transform(pose_mat, from_frame='world') start_t = time.clock() sdf_tf = sdf_2d.transform(tf, scale=1.5) end_t = time.clock() duration = end_t - start_t logging.info('2D Transform took %f sec' % (duration)) # display plt.figure() plt.subplot(1, 2, 1) sdf_2d.imshow() plt.title('Original') plt.subplot(1, 2, 2) sdf_tf.imshow() plt.title('Transformed') plt.show()
def test_2d_transform(): sdf_2d_file_name = 'data/test/sdf/medium_black_spring_clamp_optimized_poisson_texture_mapped_mesh_clean_0.csv' sf2 = sf.SdfFile(sdf_2d_file_name) sdf_2d = sf2.read() # transform pose_mat = np.matrix([[0, 1, 0, 0],[-1, 0, 0, 0],[0, 0, 1, 0],[0, 0,0,1]]) tf = tfx.transform(pose_mat, from_frame='world') start_t = time.clock() sdf_tf = sdf_2d.transform(tf, scale = 1.5) end_t = time.clock() duration = end_t - start_t logging.info('2D Transform took %f sec' %(duration)) # display plt.figure() plt.subplot(1,2,1) sdf_2d.imshow() plt.title('Original') plt.subplot(1,2,2) sdf_tf.imshow() plt.title('Transformed') plt.show()
def Interpolate(robotname): r = robot.robot(robotname) # Open and close gripper print("Starting gripper opening and closing") r.open_gripper(80) time.sleep(0.1) r.close_gripper() # Using move_cartesian_linear_interpolate # Move to a different cartesian and rotation pose start_pose = r.get_current_cartesian_position() pose1 = r.get_current_cartesian_position() pose1.position.y -= 0.03 end_pose = tfx.pose(pose1.as_tf()*tfx.transform(tfx.rotation_tb(30, -60, 30))) print("Starting move_cartesian_frame_linear_interpolation") r.move_cartesian_frame_linear_interpolation(start_pose, 0.01) time.sleep(2) r.move_cartesian_frame_linear_interpolation(end_pose, 0.01) time.sleep(2) r.move_cartesian_frame_linear_interpolation(start_pose, 0.01) # Using move_cartesian_frame print("Starting move_cartesian_frame") r.move_cartesian_frame(start_pose, interpolate=True) time.sleep(2) r.move_cartesian_frame(end_pose, interpolate=True) time.sleep(2) r.move_cartesian_frame(start_pose, interpolate=True) print start_pose print end_pose
def tool_poses(self, msg=False, tf=None): if 'tool_pose' not in self.arms.columns.levels[1]: return None poses = {} pose_data = self.arms.xs('tool_pose', axis=1, level='field') arm_names = pose_data.columns.levels[0] if tf: tf = tfx.transform(tf, pose=False) for arm in arm_names: arm_pose_data = pose_data[arm] arm_poses = [] stamps = [] for i in xrange(len(arm_pose_data)): if np.isnan(arm_pose_data.ix[i, 'x']): continue translation = arm_pose_data.ix[i, ['x', 'y', 'z']] rotation = arm_pose_data.ix[i, ['qx', 'qy', 'qz', 'qw']] if msg: pose = PoseStamped() pose.header.frame_id = frame = self.robot.ix[i, 'frame'] pose.header.stamp = tfx.stamp(arm_pose_data.index[i]).ros pose.pose.position.x = translation[0] pose.pose.position.y = translation[1] pose.pose.position.z = translation[2] pose.pose.orientation.x = rotation[0] pose.pose.orientation.y = rotation[1] pose.pose.orientation.z = rotation[2] pose.pose.orientation.w = rotation[3] else: pose = tfx.pose(translation, rotation, stamp=arm_pose_data.index[i], frame=self.robot.ix[i, 'frame'], name=arm) if tf: pose = tf * pose arm_poses.append(pose) stamps.append(arm_pose_data.index[i]) if not msg: arm_poses = pd.Series(arm_poses, index=stamps, name=arm) poses[arm] = arm_poses if not msg: return pd.DataFrame(poses) else: return poses
def write_mesh_stable_poses(self, mesh, filename, min_prob=0, vis=False): prob_mapping, cv_hull = st.compute_stable_poses(mesh), mesh.convex_hull() R_list = [] for face, p in prob_mapping.items(): if p >= min_prob: x0 = np.array(cv_hull.vertices()[face[0]]) R_list.append([p, st.compute_basis([cv_hull.vertices()[i] for i in face], cv_hull), x0]) if vis: print 'P', R_list[0][0] mv.figure() mesh.visualize() mv.axes() mv.figure() cv_hull_tf = cv_hull.transform(stf.SimilarityTransform3D(tfx.transform(R_list[0][1], np.zeros(3)))) cv_hull_tf.visualize() mv.axes() mv.show() f = open(filename[:-4] + ".stp", "w") f.write("#############################################################\n") f.write("# STP file generated by UC Berkeley Automation Sciences Lab #\n") f.write("# #\n") f.write("# Num Poses: %d" %len(R_list)) for _ in range(46 - len(str(len(R_list)))): f.write(" ") f.write(" #\n") f.write("# Min Probability: %s" %str(min_prob)) for _ in range(40 - len(str(min_prob))): f.write(" ") f.write(" #\n") f.write("# #\n") f.write("#############################################################\n") f.write("\n") # adding R matrices to .stp file pose_index = 1 for i in range(len(R_list)): f.write("p %f\n" %R_list[i][0]) f.write("r %f %f %f\n" %(R_list[i][1][0][0], R_list[i][1][0][1], R_list[i][1][0][2])) f.write(" %f %f %f\n" %(R_list[i][1][1][0], R_list[i][1][1][1], R_list[i][1][1][2])) f.write(" %f %f %f\n" %(R_list[i][1][2][0], R_list[i][1][2][1], R_list[i][1][2][2])) f.write("x0 %f %f %f\n" %(R_list[i][2][0], R_list[i][2][1], R_list[i][2][2])) f.write("\n\n")
def tool_poses(self,msg=False,tf=None): if 'tool_pose' not in self.arms.columns.levels[1]: return None poses = {} pose_data = self.arms.xs('tool_pose',axis=1,level='field') arm_names = pose_data.columns.levels[0] if tf: tf = tfx.transform(tf,pose=False) for arm in arm_names: arm_pose_data = pose_data[arm] arm_poses = [] stamps = [] for i in xrange(len(arm_pose_data)): if np.isnan(arm_pose_data.ix[i,'x']): continue translation = arm_pose_data.ix[i,['x','y','z']] rotation = arm_pose_data.ix[i,['qx','qy','qz','qw']] if msg: pose = PoseStamped() pose.header.frame_id = frame=self.robot.ix[i,'frame'] pose.header.stamp = tfx.stamp(arm_pose_data.index[i]).ros pose.pose.position.x = translation[0] pose.pose.position.y = translation[1] pose.pose.position.z = translation[2] pose.pose.orientation.x = rotation[0] pose.pose.orientation.y = rotation[1] pose.pose.orientation.z = rotation[2] pose.pose.orientation.w = rotation[3] else: pose = tfx.pose(translation,rotation,stamp=arm_pose_data.index[i],frame=self.robot.ix[i,'frame'],name=arm) if tf: pose = tf * pose arm_poses.append(pose) stamps.append(arm_pose_data.index[i]) if not msg: arm_poses = pd.Series(arm_poses,index=stamps,name=arm) poses[arm] = arm_poses if not msg: return pd.DataFrame(poses) else: return poses
def __init__(self, arms = ['L','R'], calcPosePostAdjustment=None, adjustmentInterpolation=True): self.arms = arms self.truthPose = {} self.calcPose = {} self.calcPoseAtTruth = {} self.estimatedPose = defaultdict(lambda: (None,None)) self.pre_adjustment = dict((arm,tfx.identity_tf()) for arm in self.arms) self.post_adjustment = dict((arm,tfx.identity_tf()) for arm in self.arms) self.adjustment_side = dict((arm,'post') for arm in self.arms) self.switch_adjustment_update = True self.adjustmentInterpolation = {'pre': 1, 'post': 1} if adjustmentInterpolation is True: self.adjustmentInterpolation['pre'] = self.adjustmentInterpolation['post'] = 0.5 elif isinstance(adjustmentInterpolation,dict): self.adjustmentInterpolation['pre'] = adjustmentInterpolation.get('pre',0.5) self.adjustmentInterpolation['post'] = adjustmentInterpolation.get('post',0.5) elif isinstance(adjustmentInterpolation,(list,tuple)): self.adjustmentInterpolation['pre'] = adjustmentInterpolation[0] self.adjustmentInterpolation['post'] = adjustmentInterpolation[1] else: self.adjustmentInterpolation['pre'] = self.adjustmentInterpolation['post'] = adjustmentInterpolation self.estimateFromImage = dict() self.calcPosePostAdjustment = dict((arm,tfx.identity_tf()) for arm in self.arms) if calcPosePostAdjustment: for k,v in calcPosePostAdjustment: self.calcPosePostAdjustment[k] = tfx.transform(v,copy=True) self.est_pose_pub = {} self.pose_error_pub = {} self.pre_adj_pub = {} self.post_adj_pub = {} for arm in self.arms: rospy.Subscriber(Constants.GripperTape.Topic+'_'+arm, PoseStamped, partial(self._truthCallback,arm)) self.est_pose_pub[arm] = rospy.Publisher('estimated_gripper_pose_%s'%arm, PoseStamped) self.pose_error_pub[arm] = rospy.Publisher('estimated_gripper_pose_error_%s'%arm, PoseStamped) self.pre_adj_pub[arm] = rospy.Publisher('estimated_gripper_pose_pre_adjument_%s'%arm, TransformStamped) self.post_adj_pub[arm] = rospy.Publisher('estimated_gripper_pose_post_adjument_%s'%arm, TransformStamped) rospy.Subscriber(Constants.RavenTopics.RavenState, RavenState, self._ravenStateCallback)
def write_mesh_stable_poses(self, mesh, filename, min_prob=0, vis=False): prob_mapping, cv_hull = st.compute_stable_poses(mesh), mesh.convex_hull() R_list = [] for face, p in prob_mapping.items(): if p >= min_prob: R_list.append([p, st.compute_basis([cv_hull.vertices()[i] for i in face])]) if vis: print 'P', R_list[0][0] mv.figure() mesh.visualize() mv.axes() mv.figure() cv_hull_tf = cv_hull.transform(stf.SimilarityTransform3D(tfx.transform(R_list[0][1], np.zeros(3)))) cv_hull_tf.visualize() mv.axes() mv.show() f = open(filename[:-4] + ".stp", "w") f.write("#############################################################\n") f.write("# STP file generated by UC Berkeley Automation Sciences Lab #\n") f.write("# #\n") f.write("# Num Poses: %d" %len(R_list)) for _ in range(46 - len(str(len(R_list)))): f.write(" ") f.write(" #\n") f.write("# Min Probability: %s" %str(min_prob)) for _ in range(40 - len(str(min_prob))): f.write(" ") f.write(" #\n") f.write("# #\n") f.write("#############################################################\n") f.write("\n") # adding R matrices to .stp file pose_index = 1 for i in range(len(R_list)): f.write("p %f\n" %R_list[i][0]) f.write("r %f %f %f\n" %(R_list[i][1][0][0], R_list[i][1][0][1], R_list[i][1][0][2])) f.write(" %f %f %f\n" %(R_list[i][1][1][0], R_list[i][1][1][1], R_list[i][1][1][2])) f.write(" %f %f %f\n" %(R_list[i][1][2][0], R_list[i][1][2][1], R_list[i][1][2][2])) f.write("\n\n")
def sample(self, size=1): """ Sample |size| random variables from the model """ samples = [] for i in range(size): # sample random pose xi = self.r_xi_rv_.rvs(size=1) S_xi = skew(xi) R = scipy.linalg.expm(S_xi).dot(self.obj_.tf.rotation) s = self.s_rv_.rvs(size=1)[0] t = self.t_rv_.rvs(size=1) sample_tf = stf.SimilarityTransform3D(tfx.transform(R.T, t), s) # transform object by pose obj_sample = self.obj_.transform(sample_tf) samples.append(obj_sample) # not a list if only 1 sample if size == 1: return samples[0] return samples
def test_3d_transform(): np.random.seed(100) sdf_3d_file_name = 'data/test/sdf/Co_clean_dim_25.sdf' s = sf.SdfFile(sdf_3d_file_name) sdf_3d = s.read() # transform pose_mat = np.matrix([[0, 1, 0, 0], [-1, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) tf = tfx.transform(pose_mat, from_frame='world') tf = tfx.random_tf() tf.position = 0.01 * np.random.rand(3) start_t = time.clock() s_tf = stf.SimilarityTransform3D(tf, scale=1.2) sdf_tf = sdf_3d.transform(s_tf) end_t = time.clock() duration = end_t - start_t logging.info('3D Transform took %f sec' % (duration)) logging.info('Transformed resolution %f' % (sdf_tf.resolution)) start_t = time.clock() sdf_tf_d = sdf_3d.transform(s_tf, detailed=True) end_t = time.clock() duration = end_t - start_t logging.info('Detailed 3D Transform took %f sec' % (duration)) logging.info('Transformed detailed resolution %f' % (sdf_tf_d.resolution)) # display plt.figure() sdf_3d.scatter() plt.title('Original') plt.figure() sdf_tf.scatter() plt.title('Transformed') plt.figure() sdf_tf_d.scatter() plt.title('Detailed Transformed') plt.show()
def test_3d_transform(): np.random.seed(100) sdf_3d_file_name = 'data/test/sdf/Co_clean_dim_25.sdf' s = sf.SdfFile(sdf_3d_file_name) sdf_3d = s.read() # transform pose_mat = np.matrix([[0, 1, 0, 0],[-1, 0, 0, 0],[0, 0, 1, 0],[0, 0,0,1]]) tf = tfx.transform(pose_mat, from_frame='world') tf = tfx.random_tf() tf.position = 0.01 * np.random.rand(3) start_t = time.clock() s_tf = stf.SimilarityTransform3D(tf, scale = 1.2) sdf_tf = sdf_3d.transform(s_tf) end_t = time.clock() duration = end_t - start_t logging.info('3D Transform took %f sec' %(duration)) logging.info('Transformed resolution %f' %(sdf_tf.resolution)) start_t = time.clock() sdf_tf_d = sdf_3d.transform(s_tf, detailed = True) end_t = time.clock() duration = end_t - start_t logging.info('Detailed 3D Transform took %f sec' %(duration)) logging.info('Transformed detailed resolution %f' %(sdf_tf_d.resolution)) # display plt.figure() sdf_3d.scatter() plt.title('Original') plt.figure() sdf_tf.scatter() plt.title('Transformed') plt.figure() sdf_tf_d.scatter() plt.title('Detailed Transformed') plt.show()
def __init__(self, arm, sim, tool_to_camera=None, height=480., width=640., focal_length=.01, fx=525., fy=525., cx=319.5, cy=239.5, min_range=.3, max_range=1.5): #fx=640.*2., fy=480.*2., cx=640./2.+0.5, cy=480./2.+0.5, max_range=1.5): self.arm = arm self.sim = sim self.tool_to_camera = tool_to_camera if tool_to_camera is not None else tfx.transform(wrist_to_hand) self.height = height self.width = width self.fx, self.fy, self.cx, self.cy = fx, fy, cx, cy self.P = np.array([[fx, 0, cx], [ 0, fy, cy], [ 0, 0, 1]]) self.focal_length = focal_length # TODO: not sure if needed self.min_range = min_range self.max_range = max_range self.height_m = focal_length*(height/fy) self.width_m = focal_length*(width/fx)
roslib.load_manifest('tfx') import tfx import numpy as np from matplotlib import pyplot as plt from matplotlib import delaunay import time import random from geometry import geometry2d, geometry3d from pr2_sim import simulator, arm from utils import utils wrist_to_hand = tfx.transform( (-0.111015481946, -0.0160376173344, -0.0735991062825), (0.531327739419, 0.463547417, 0.524778479206, 0.476888009158)) class RelativePyramid: def __init__(self, points, point_frames): assert (len(points) == 3 and len(point_frames) == 3) self.points = points self.point_frames = point_frames def construct_pyramid(self, cam): cam_pose = np.array(cam.get_pose().matrix) cam_rot, cam_trans = cam_pose[:3, :3], cam_pose[:3, 3] abs_points3d = list() for pt, frame in zip(self.points, self.point_frames):
def kinect_transform_publisher(): global head_pose, hand_pose head_pose = None hand_pose = None dummy = tfx.transform([0, 0, 0]) #pub = rospy.Publisher('kinect_transform', ) pub = tfx.TransformBroadcaster() rospy.init_node('kinect_transform_publisher', anonymous=True) rospy.Subscriber("/hand_kinect_ar_kinect_pose", ARMarkers, hand_callback) rospy.Subscriber("/head_kinect_ar_kinect_pose", ARMarkers, head_callback) transformer = tfx.TransformListener() r = rospy.Rate(5) #head_pose = True # REMOVE THIS #head_pose = False # REMOVE THIS i = 0 transforms = [] while not rospy.is_shutdown() and i < 500: if head_pose and hand_pose: head_transform = tfx.transform(head_pose, parent='camera_rgb_optical_frame', child='ar_frame') #.as_transform() hand_transform = tfx.transform(hand_pose, parent='hand_kinect_optical_frame', child='ar_frame') #.as_transform() #head_to_ar = tfx.transform(transformer.lookupTransform('ar_frame', 'camera_rgb_optical_frame', rospy.Time()), parent='camera_rgb_optical_frame', child='ar_frame') #ar_to_hand = tfx.transform(transformer.lookupTransform('hand_kinect_optical_frame', 'ar_frame', rospy.Time()), parent='ar_frame', child='hand_kinect_optical_frame') head_to_hand = tfx.transform(head_transform.matrix * hand_transform.inverse().matrix, parent='camera_rgb_optical_frame', child='hand_kinect_optical_frame') #head_to_hand = tfx.transform(head_to_ar.inverse().matrix * ar_to_hand.matrix, parent='camera_rgb_optical_frame', child='hand_kinect_optical_frame') #rospy.loginfo(head_transform) #rospy.loginfo(hand_transform.inverse()) #rospy.loginfo(head_to_ar) #rospy.loginfo(ar_to_hand) #print head_to_hand wrist_to_head = tfx.lookupTransform('r_gripper_tool_frame', 'camera_rgb_optical_frame') #wrist_to_head = tfx.transform(transformer.lookupTransform('r_gripper_tool_frame', 'camera_rgb_optical_frame', rospy.Time()), child='camera_rgb_optical_frame', parent='r_gripper_tool_frame') wrist_to_hand = tfx.transform(wrist_to_head.matrix * head_to_hand.matrix, parent='r_gripper_tool_frame', child='hand_kinect_optical_frame') #print wrist_to_head print wrist_to_hand print #rospy.loginfo(wrist_to_head) #rospy.loginfo(wrist_to_hand) #pub.sendTransform(wrist_to_hand.position, wrist_to_hand.rotation, rospy.Time(), wrist_to_hand.child, wrist_to_hand.parent) #pub.sendTransform(head_to_hand.position, head_to_hand.rotation, rospy.Time(), head_to_hand.child, head_to_hand.parent) transforms.append(wrist_to_hand) i += 1 else: if head_pose is None: print('Head pose is None') if hand_pose is None: print('Hand pose is None') print('') r.sleep() transform = transforms[0] for i in range(1, len(transforms)): transform = transform.interpolate(transforms[i], 1 / len(transforms)) transform.child = "camera_rgb_optical_frame" print transform rospack = rospkg.RosPack() os.chdir(rospack.get_path("kinect_transform")) s = '<launch>\n' s += '<node pkg="tf" type="static_transform_publisher" name="static3" args="' for value in transform.position.list + transform.quaternion.list: s += str(value) + ' ' s += transform.parent + ' ' + transform.child + ' ' s += '100" />\n' s += '</launch>' try: f = open("launch/wrist_to_hand.launch", "w") f.write(s) finally: f.close()
def kinect_transform_publisher(): global head_pose, hand_pose head_pose = None hand_pose = None dummy = tfx.transform([0,0,0]) #pub = rospy.Publisher('kinect_transform', ) pub = tfx.TransformBroadcaster() rospy.init_node('kinect_transform_publisher', anonymous=True) rospy.Subscriber("/hand_kinect_ar_kinect_pose", ARMarkers, hand_callback) rospy.Subscriber("/head_kinect_ar_kinect_pose", ARMarkers, head_callback) transformer = tfx.TransformListener() r = rospy.Rate(5) #head_pose = True # REMOVE THIS #head_pose = False # REMOVE THIS i = 0 transforms = [] while not rospy.is_shutdown() and i < 500: if head_pose and hand_pose: head_transform = tfx.transform(head_pose, parent='camera_rgb_optical_frame', child='ar_frame')#.as_transform() hand_transform = tfx.transform(hand_pose, parent='hand_kinect_optical_frame', child='ar_frame')#.as_transform() #head_to_ar = tfx.transform(transformer.lookupTransform('ar_frame', 'camera_rgb_optical_frame', rospy.Time()), parent='camera_rgb_optical_frame', child='ar_frame') #ar_to_hand = tfx.transform(transformer.lookupTransform('hand_kinect_optical_frame', 'ar_frame', rospy.Time()), parent='ar_frame', child='hand_kinect_optical_frame') head_to_hand = tfx.transform(head_transform.matrix * hand_transform.inverse().matrix, parent='camera_rgb_optical_frame', child='hand_kinect_optical_frame') #head_to_hand = tfx.transform(head_to_ar.inverse().matrix * ar_to_hand.matrix, parent='camera_rgb_optical_frame', child='hand_kinect_optical_frame') #rospy.loginfo(head_transform) #rospy.loginfo(hand_transform.inverse()) #rospy.loginfo(head_to_ar) #rospy.loginfo(ar_to_hand) #print head_to_hand wrist_to_head = tfx.lookupTransform('r_gripper_tool_frame', 'camera_rgb_optical_frame') #wrist_to_head = tfx.transform(transformer.lookupTransform('r_gripper_tool_frame', 'camera_rgb_optical_frame', rospy.Time()), child='camera_rgb_optical_frame', parent='r_gripper_tool_frame') wrist_to_hand = tfx.transform(wrist_to_head.matrix * head_to_hand.matrix, parent='r_gripper_tool_frame', child='hand_kinect_optical_frame') #print wrist_to_head print wrist_to_hand print #rospy.loginfo(wrist_to_head) #rospy.loginfo(wrist_to_hand) #pub.sendTransform(wrist_to_hand.position, wrist_to_hand.rotation, rospy.Time(), wrist_to_hand.child, wrist_to_hand.parent) #pub.sendTransform(head_to_hand.position, head_to_hand.rotation, rospy.Time(), head_to_hand.child, head_to_hand.parent) transforms.append(wrist_to_hand) i += 1 else: if head_pose is None: print('Head pose is None') if hand_pose is None: print('Hand pose is None') print('') r.sleep() transform = transforms[0] for i in range(1, len(transforms)): transform = transform.interpolate(transforms[i], 1 / len(transforms)) transform.child = "camera_rgb_optical_frame" print transform rospack = rospkg.RosPack() os.chdir(rospack.get_path("kinect_transform")) s = '<launch>\n' s += '<node pkg="tf" type="static_transform_publisher" name="static3" args="' for value in transform.position.list + transform.quaternion.list: s += str(value) + ' ' s += transform.parent + ' ' + transform.child + ' ' s += '100" />\n' s += '</launch>' try: f = open("launch/wrist_to_hand.launch", "w") f.write(s) finally: f.close()
import roslib roslib.load_manifest('tfx') import tfx import numpy as np from matplotlib import pyplot as plt from matplotlib import delaunay import time import random from geometry import geometry2d, geometry3d from pr2_sim import simulator, arm from utils import utils wrist_to_hand = tfx.transform((-0.111015481946, -0.0160376173344, -0.0735991062825), (0.531327739419, 0.463547417, 0.524778479206, 0.476888009158)) class RelativePyramid: def __init__(self, points, point_frames): assert(len(points) == 3 and len(point_frames) == 3) self.points = points self.point_frames = point_frames def construct_pyramid(self, cam): cam_pose = np.array(cam.get_pose().matrix) cam_rot, cam_trans = cam_pose[:3,:3], cam_pose[:3,3] abs_points3d = list() for pt, frame in zip(self.points, self.point_frames): assert(frame.count('base_link') > 0 or frame.count('camera') > 0)
def actual_world_to_ik_world(armId): if armId == GOLD_ARM_ID: return tfx.transform([[0, 1, 0], [-1, 0, 0], [0, 0, 1]]) else: return tfx.transform([[0, -1, 0], [1, 0, 0], [0, 0, 1]], (-0.149, 0.005, -0.006))
GRASP1 = 6 GRASP2 = 7 YAW = 8 GRASP = 9 A12 = 1.30899694 #Link1 - 75deg in RAD A23 = 0.907571211 #Link2 - 52deg in RAD - was set to 60 GOLD_ARM_ID = 'L' GREEN_ARM_ID = 'R' RAD2DEG = 180. / pi DEG2RAD = pi / 180. TOOL_POSE_AXES_TRANSFORM = tfx.transform((1, 0, 0, 0, -1, 0, 0, 0, -1)) SHOULDER_OFFSET_GOLD = atan(0.3471 / 0.9014) #from original URDF TOOL_ROT_OFFSET_GOLD = -pi / 2 WRIST_OFFSET_GOLD = 0 SHOULDER_OFFSET_GREEN = atan(0.3471 / 0.9014) #from original URDF TOOL_ROT_OFFSET_GREEN = -pi / 2 WRIST_OFFSET_GREEN = 0 THETA_12 = -A12 THETA_23 = -A23 DW = 0.012 Z = lambda theta, d: tfx.transform([[cos(float(theta)), -sin(float( theta)), 0], [sin(theta), cos(float(theta)), 0], [0, 0, 1]], (0, 0, d))
def listener(self): rospy.init_node('robo_registration', anonymous=True) # dvrk_psm1/joint_position_cartesian is right arm pose_topic = "" if self.arm == "right": pose_topic = "dvrk_psm1/joint_position_cartesian" else: pose_topic = "dvrk_psm2/joint_position_cartesian" rospy.Subscriber(pose_topic, PoseStamped, self.record_pose_callback) rospy.Subscriber("/BC/chessboard_pose", PoseStamped, self.get_transform_callback) save_camera_transform = raw_input("Do you want to save a new camera transform? (yes/no) ") if save_camera_transform == "yes": while not self.have_camera_transform and not rospy.is_shutdown(): print("Waiting for camera transform") rospy.sleep(0.5) print("Saving camera transform in file") f = open("/home/davinci2/catkin_ws/src/davinci_vision/launch/BC_registration/camera_frame.p", "wb") pickle.dump(self.camera_transform, f) f.close() else: f = open("/home/davinci2/catkin_ws/src/davinci_vision/launch/BC_registration/camera_frame.p", "rb") self.camera_transform = pickle.load(f) f.close() raw_input("Remove chessboard; press any key when done") cols = int(raw_input("Enter number of columns on registration block: ")) rows = int(raw_input("Enter number of rows on registration block: ")) dimensions = [cols, rows] #will have to do both grippers print("Please start at top left corner and go row by row, moving from left to right. Use " + self.arm + \ " gripper") print("First coordinate is x, second is y; (0, 0) is top left corner") # cols_even = cols % 2 == 0 # rows_even = rows % 2 == 0 # wrist_poses = [] for j in range(rows): for i in range(cols): command = raw_input("Press r to record current pose (cell " + str(i) + ", " + str(j) + "): ") if command == "r": self.recording_pose = True print("Pose recorded!") # wrist_poses.append(tfx.lookupTransform("two_tool_wrist_sca_shaft_link", "two_remote_center_link").msg.PoseStamped()) # b = tfx.pose(transform) # b.name = None # b.as_tf() * self.pose_data[0] #LOLMAGICNUMBER offset = tfx.transform([0, 0, -0.00483], [[1, 0, 0], [0, 1, 0], [0, 0, 1]]) trans_data = [tfx.pose(tfx.pose(pose).as_tf() * offset).msg.PoseStamped() for pose in self.pose_data] self.pose_data = trans_data # while not self.done_recording: # command = raw_input("Press r to record current pose or q to finish: ") # if command == "r": # self.recording_pose = True # print("Pose recorded!") # elif command == "q": # self.done_recording = True # b = tfx.pose(transform) # b.name = None # b = b.as_tf() # transformed_pose_data = [b * pose for pose in self.pose_data] # transform = tfx.lookupTransform("two_tool_base_link", "two_tool_middle_link") # b = tfx.pose(transform) # b.name = None # b = b.as_tf() # transformed_pose_data = [(b * pose) for pose in self.pose_data] f = open('pose_data_' + self.arm + '.p', 'wb') pickle.dump([dimensions, self.pose_data], f) f.close() # f = open('pose_data_wrist_' + self.arm + '.p', 'wb') # pickle.dump([dimensions, wrist_poses], f) # f.close() self.calculate(dimensions) # self.pose_data = wrist_poses # self.calculate(dimensions) rospy.spin()
def execute_raster_tilted(self, theta, direction): """ Direction should be 1 for L2R and -1 for R2L""" """ Linearly interpolates through a series of palpation points """ print('hi') steps = 12 poses = [] origin = np.hstack(np.array(self.tissue_pose.position)) frame = np.array(self.tissue_pose.orientation.matrix) u, v, w = frame.T[0], frame.T[1], frame.T[2] rotation_matrix = np.array([v, u, -w]).transpose() # frame = tfx.pose(tfx.pose(frame_og).as_tf()*tfx.transform(tfx.tb_angles(roll=10, pitch=0, yaw=0))).rotation.matrix # u, v, w = frame.T[0], frame.T[1], frame.T[2] # rot1 = np.array([v, u, -w]).transpose() # frame = tfx.pose(tfx.pose(frame_og).as_tf()*tfx.transform(tfx.tb_angles(roll=0, pitch=10, yaw=0))).rotation.matrix # u, v, w = frame.T[0], frame.T[1], frame.T[2] # rot2 = np.array([v, u, -w]).transpose() # frame = tfx.pose(tfx.pose(frame_og).as_tf()*tfx.transform(tfx.tb_angles(roll=0, pitch=0, yaw=10))).rotation.matrix # u, v, w = frame.T[0], frame.T[1], frame.T[2] # rot3 = np.array([v, u, -w]).transpose() # import IPython; IPython.embed() dy = self.tissue_length/(steps) z = self.probe_offset print('what"s') # pick up tool # self.pick_up_tool() print('up') self.probe_stop_reset() pose = tfx.pose(origin, rotation_matrix, frame=self.tissue_pose.frame) # # pose1 = tfx.pose(pose.as_tf()*tfx.transform(tfx.tb_angles(roll=15, pitch=0, yaw=0))) #apparently this tilts head of probe toward base of arms. wait no apparently this tilts head of probe toward monitors? (to the right) this is probably the direction that the probe constrains rotation in rotation_matrix = tfx.pose(pose.as_tf()*tfx.transform(tfx.tb_angles(roll=0, pitch=theta*direction, yaw=0))).rotation.matrix RASTER_SPEED = 0.005 INAIR_SPEED = 0.03 TEST_OFFSET = 0 TEST_OFFSET2 = 0.3 # steps = 6 for i in range(steps): if i == 0: continue # offset = np.dot(frame, np.array([i*dy, 0.0, z+0.03])) # pose = tfx.pose(origin+offset, rotation_matrix, frame=self.tissue_pose.frame) # # # pose1 = tfx.pose(pose.as_tf()*tfx.transform(tfx.tb_angles(roll=15, pitch=0, yaw=0))) #apparently this tilts head of probe toward base of arms. wait no apparently this tilts head of probe toward monitors? (to the right) this is probably the direction that the probe constrains rotation in # rotation_matrix = tfx.pose(pose.as_tf()*tfx.transform(tfx.tb_angles(roll=0, pitch=theta*direction, yaw=0))).rotation.matrix #tilted even more toward monitors? what? also a bit towards base. why are roll & pitch the same o.o joint limits? moves more than pose1. # should be done before for loop? lol # pose1 = tfx.pose(pose.as_tf()*tfx.transform(tfx.tb_angles(roll=10, pitch=0, yaw=0))) #away from arms and to the left # pose2 = tfx.pose(pose.as_tf()*tfx.transform(tfx.tb_angles(roll=0, pitch=10, yaw=0))) #away from arms and to the left # pose3 = tfx.pose(pose.as_tf()*tfx.transform(tfx.tb_angles(roll=0, pitch=0, yaw=10))) #away from arms and to the left # pose4 = tfx.pose(origin+offset, rot1) #so apparently these don't work # pose5 = tfx.pose(origin+offset, rot2) # pose6 = tfx.pose(origin+offset, rot3) # print('og pose') # self.psm1.move_cartesian_frame_linear_interpolation(pose, SPEED, False) # import IPython; IPython.embed() # print('pose1') # self.psm1.move_cartesian_frame_linear_interpolation(pose1, SPEED, False) # import IPython; IPython.embed() # print('pose2') # self.psm1.move_cartesian_frame_linear_interpolation(pose2, SPEED, False) # import IPython; IPython.embed() # print('pose3') # self.psm1.move_cartesian_frame_linear_interpolation(pose3, SPEED, False) # import IPython; IPython.embed() if direction == 1: offset = np.dot(frame, np.array([i*dy, 0.0, z+0.02+TEST_OFFSET])) pose = tfx.pose(origin+offset, rotation_matrix, frame=self.tissue_pose.frame) self.psm1.move_cartesian_frame_linear_interpolation(pose, INAIR_SPEED, False) a = str(self.psm1.get_current_joint_position()) print(a) # import IPython; IPython.embed() # print('og pose') # self.psm1.move_cartesian_frame_linear_interpolation(pose, 0.01, False) # import IPython; IPython.embed() # print('pose 1') # self.psm1.move_cartesian_frame_linear_interpolation(pose1, 0.005, False) # IPython.embed() # print('pose 2') # self.psm1.move_cartesian_frame_linear_interpolation(pose2, 0.005, False) # IPython.embed() # print('pose 3') # self.psm1.move_cartesian_frame_linear_interpolation(pose3, 0.005, False) # IPython.embed() # print('pose 4') # self.psm1.move_cartesian_frame_linear_interpolation(pose4, 0.005, False) # IPython.embed() # print('pose 5') # self.psm1.move_cartesian_frame_linear_interpolation(pose5, 0.005, False) # IPython.embed() # print('pose 6') # self.psm1.move_cartesian_frame_linear_interpolation(pose6, 0.005, False) # IPython.embed() offset = np.dot(frame, np.array([i*dy, 0.0, z+TEST_OFFSET])) pose = tfx.pose(origin+offset, rotation_matrix, frame=self.tissue_pose.frame) self.psm1.move_cartesian_frame_linear_interpolation(pose, INAIR_SPEED, False) b = str(self.psm1.get_current_joint_position()) print(b) # start recording data rospy.sleep(0.2) self.probe_start() offset = np.dot(frame, np.array([i*dy, self.tissue_width*(0.95+TEST_OFFSET2), z+TEST_OFFSET])) pose = tfx.pose(origin+offset, rotation_matrix, frame=self.tissue_pose.frame) self.psm1.move_cartesian_frame_linear_interpolation(pose, RASTER_SPEED, False) c = str(self.psm1.get_current_joint_position()) print(c) # pause recording data rospy.sleep(0.2) self.probe_pause() print(self.probe_data) offset = np.dot(frame, np.array([i*dy, self.tissue_width*(0.95+TEST_OFFSET2), z+0.02+TEST_OFFSET])) pose = tfx.pose(origin+offset, rotation_matrix, frame=self.tissue_pose.frame) self.psm1.move_cartesian_frame_linear_interpolation(pose, INAIR_SPEED, False) d = str(self.psm1.get_current_joint_position()) print(d) # offset = np.dot(frame, np.array([i*dy+0.04, 0, z+0.03])) # pose = tfx.pose(origin+offset, rotation_matrix, frame=self.tissue_pose.frame) # self.psm1.move_cartesian_frame_linear_interpolation(pose, SPEED, False) # e = str(self.psm1.get_current_joint_position()) # print(e) # import IPython; IPython.embed() else: offset = np.dot(frame, np.array([i*dy, self.tissue_width*0.95, z+0.02])) pose = tfx.pose(origin+offset, rotation_matrix, frame=self.tissue_pose.frame) self.psm1.move_cartesian_frame_linear_interpolation(pose, INAIR_SPEED, False) offset = np.dot(frame, np.array([i*dy, self.tissue_width*0.95, z])) pose = tfx.pose(origin+offset, rotation_matrix, frame=self.tissue_pose.frame) self.psm1.move_cartesian_frame_linear_interpolation(pose, INAIR_SPEED, False) # start recording data rospy.sleep(0.2) self.probe_start() offset = np.dot(frame, np.array([i*dy, 0.0, z])) pose = tfx.pose(origin+offset, rotation_matrix, frame=self.tissue_pose.frame) self.psm1.move_cartesian_frame_linear_interpolation(pose, RASTER_SPEED, False) # pause recording data rospy.sleep(0.2) self.probe_pause() print(self.probe_data) offset = np.dot(frame, np.array([i*dy, 0.0, z+0.02])) pose = tfx.pose(origin+offset, rotation_matrix, frame=self.tissue_pose.frame) self.psm1.move_cartesian_frame_linear_interpolation(pose, INAIR_SPEED, False) print(self.probe_data) self.probe_save("probe_data_newdvrk_po" + str(self.probe_offset) + "_s" + str(RASTER_SPEED) + "_t" + str(theta) + "_d" + str(direction) + ".p")
def actual_world_to_ik_world(armId): if armId == GOLD_ARM_ID: return tfx.transform([[0,1,0],[ -1,0,0],[ 0,0,1]]) else: return tfx.transform([[0,-1,0],[1,0,0], [0,0,1]],(-0.149,0.005,-0.006))
WRIST =5 GRASP1 =6 GRASP2 =7 YAW =8 GRASP =9 A12 = 1.30899694; #Link1 - 75deg in RAD A23 = 0.907571211 #Link2 - 52deg in RAD - was set to 60 GOLD_ARM_ID = 'L' GREEN_ARM_ID = 'R' RAD2DEG = 180. / pi DEG2RAD = pi / 180. TOOL_POSE_AXES_TRANSFORM = tfx.transform((1,0,0, 0,-1,0, 0,0,-1)); SHOULDER_OFFSET_GOLD = atan(0.3471/0.9014) #from original URDF TOOL_ROT_OFFSET_GOLD = -pi/2 WRIST_OFFSET_GOLD = 0 SHOULDER_OFFSET_GREEN = atan(0.3471/0.9014)#from original URDF TOOL_ROT_OFFSET_GREEN = -pi/2 WRIST_OFFSET_GREEN = 0 THETA_12 = -A12 THETA_23 = -A23 DW = 0.012 Z = lambda theta,d: tfx.transform([[cos(float(theta)),-sin(float(theta)),0], [sin(theta),cos(float(theta)),0], [0,0,1]],(0,0,d)) X = lambda alpha,a: tfx.transform([[1,0,0], [0,cos(float(alpha)),-sin(float(alpha))], [0,sin(float(alpha)),cos(float(alpha))]],(a,0,0))
def listener(self): rospy.init_node('robo_registration', anonymous=True) # dvrk_psm1/joint_position_cartesian is right arm pose_topic = "" if self.arm == "right": pose_topic = "dvrk_psm1/joint_position_cartesian" else: pose_topic = "dvrk_psm2/joint_position_cartesian" rospy.Subscriber(pose_topic, PoseStamped, self.record_pose_callback) rospy.Subscriber("/BC/chessboard_pose", PoseStamped, self.get_transform_callback) save_camera_transform = raw_input( "Do you want to save a new camera transform? (yes/no) ") if save_camera_transform == "yes": while not self.have_camera_transform and not rospy.is_shutdown(): print("Waiting for camera transform") rospy.sleep(0.5) print("Saving camera transform in file") f = open( "/home/davinci2/catkin_ws/src/davinci_vision/launch/BC_registration/camera_frame.p", "wb") pickle.dump(self.camera_transform, f) f.close() else: f = open( "/home/davinci2/catkin_ws/src/davinci_vision/launch/BC_registration/camera_frame.p", "rb") self.camera_transform = pickle.load(f) f.close() raw_input("Remove chessboard; press any key when done") cols = int( raw_input("Enter number of columns on registration block: ")) rows = int(raw_input("Enter number of rows on registration block: ")) dimensions = [cols, rows] #will have to do both grippers print("Please start at top left corner and go row by row, moving from left to right. Use " + self.arm + \ " gripper") print("First coordinate is x, second is y; (0, 0) is top left corner") # cols_even = cols % 2 == 0 # rows_even = rows % 2 == 0 # wrist_poses = [] for j in range(rows): for i in range(cols): command = raw_input("Press r to record current pose (cell " + str(i) + ", " + str(j) + "): ") if command == "r": self.recording_pose = True print("Pose recorded!") # wrist_poses.append(tfx.lookupTransform("two_tool_wrist_sca_shaft_link", "two_remote_center_link").msg.PoseStamped()) # b = tfx.pose(transform) # b.name = None # b.as_tf() * self.pose_data[0] #LOLMAGICNUMBER offset = tfx.transform([0, 0, -0.00483], [[1, 0, 0], [0, 1, 0], [0, 0, 1]]) trans_data = [ tfx.pose(tfx.pose(pose).as_tf() * offset).msg.PoseStamped() for pose in self.pose_data ] self.pose_data = trans_data # while not self.done_recording: # command = raw_input("Press r to record current pose or q to finish: ") # if command == "r": # self.recording_pose = True # print("Pose recorded!") # elif command == "q": # self.done_recording = True # b = tfx.pose(transform) # b.name = None # b = b.as_tf() # transformed_pose_data = [b * pose for pose in self.pose_data] # transform = tfx.lookupTransform("two_tool_base_link", "two_tool_middle_link") # b = tfx.pose(transform) # b.name = None # b = b.as_tf() # transformed_pose_data = [(b * pose) for pose in self.pose_data] f = open('pose_data_' + self.arm + '.p', 'wb') pickle.dump([dimensions, self.pose_data], f) f.close() # f = open('pose_data_wrist_' + self.arm + '.p', 'wb') # pickle.dump([dimensions, wrist_poses], f) # f.close() self.calculate(dimensions) # self.pose_data = wrist_poses # self.calculate(dimensions) rospy.spin()