def execute(self, userdata): base_T_clone = tfu.tf_as_matrix(self.robot.tf_listener.lookupTransform(self.base_frame, self.frame_to_clone, rospy.Time(0))) #print 'BASE FRAME', self.base_frame, 'FRAME TO CLONE', self.frame_to_clone #print base_T_clone pose = tfu.stamp_pose(tfu.mat_to_pose(base_T_clone), self.base_frame) self.broadcast_transform_srv(self.new_frame_name, pose) return 'succeeded'
def execute(self, userdata): base_T_clone = tfu.tf_as_matrix( self.robot.tf_listener.lookupTransform(self.base_frame, self.frame_to_clone, rospy.Time(0))) pose = tfu.stamp_pose(tfu.mat_to_pose(base_T_clone), self.base_frame) self.broadcast_transform_srv(self.new_frame_name, pose) return 'succeeded'
def get_current_pose(self): frame = str(self.frameline.text()) self.tf_listener.waitForTransform(frame, self.robot_frame_name, rospy.Time(), rospy.Duration(2.)) p_base = tfu.transform(frame, self.robot_frame_name, self.tf_listener) \ * tfu.tf_as_matrix(([0., 0., 0., 1.], tr.quaternion_from_euler(0,0,0))) t, r = tfu.matrix_as_tf(p_base) x = t[0] y = t[1] theta = tr.euler_from_quaternion(r)[2] print x,y,theta self.xline.setText(str(x)) self.yline.setText(str(y)) self.tline.setText(str(math.degrees(theta)))
def get_current_pose(self): frame = str(self.frameline.text()) self.tf_listener.waitForTransform(frame, self.robot_frame_name, rospy.Time(), rospy.Duration(2.)) p_base = tfu.transform(frame, self.robot_frame_name, self.tf_listener) \ * tfu.tf_as_matrix(([0., 0., 0., 1.], tr.quaternion_from_euler(0,0,0))) t, r = tfu.matrix_as_tf(p_base) x = t[0] y = t[1] theta = tr.euler_from_quaternion(r)[2] print x, y, theta self.xline.setText(str(x)) self.yline.setText(str(y)) self.tline.setText(str(math.degrees(theta)))
def get_current_pose(self): frame_described_in = str(self.frameline.text()) left = ('Left' == str(self.arm_box.currentText())) right = False if not left: right = True arm_tip_frame = LinearMoveTool.RIGHT_TIP else: arm_tip_frame = LinearMoveTool.LEFT_TIP self.tf_listener.waitForTransform(frame_described_in, arm_tip_frame, rospy.Time(), rospy.Duration(2.)) p_arm = tfu.tf_as_matrix(self.tf_listener.lookupTransform(frame_described_in, arm_tip_frame, rospy.Time(0))) * tr.identity_matrix() trans, rotation = tr.translation_from_matrix(p_arm), tr.quaternion_from_matrix(p_arm) for value, vr in zip(trans, [self.xline, self.yline, self.zline]): vr.setText(str(value)) for value, vr in zip(tr.euler_from_quaternion(rotation), [self.phi_line, self.theta_line, self.psi_line]): vr.setText(str(np.degrees(value)))
def action_cb(self, msg): rospy.loginfo('message that we got:\n' + str(msg)) self.controller_manager.cart_mode(self.arm) trans_tolerance = msg.trans_tolerance #rospy.get_param("~translation_tolerance") time_out = msg.time_out #rospy.get_param("~timeout") self.rot_tolerance = math.radians( rospy.get_param("~rotation_tolerance")) #self._wait_for_pose_message() success = False r = rospy.Rate(100) goal_ps = msg.goal #relative_movement = msg.relative trans_vel = msg.trans_vel rot_vel = msg.rot_vel if trans_vel <= 0: trans_vel = .02 if rot_vel <= 0: rot_vel = pi / 20. tstart = rospy.get_time() tmax = tstart + time_out #self.controller_manager = ControllerManager() rospy.loginfo('Goal is x %f y %f z %f in %s' % (goal_ps.pose.position.x, goal_ps.pose.position.y, goal_ps.pose.position.z, goal_ps.header.frame_id)) goal_torso = change_pose_stamped_frame(self.tf, goal_ps, 'torso_lift_link') rospy.loginfo('BEFORE TIP Goal is x %f y %f z %f in %s' % (goal_torso.pose.position.x, goal_torso.pose.position.y, goal_torso.pose.position.z, goal_torso.header.frame_id)) rospy.loginfo('Tool Frame %s Controller Frame %s' % (self.tool_frame, self.controller_frame)) tll_T_tip = pose_to_mat(goal_torso.pose) tip_T_w = tf_as_matrix( self.tf.lookupTransform(self.tool_frame, self.controller_frame, rospy.Time(0))) #print 'TRANSFORM', tip_T_w tll_T_w = tll_T_tip * tip_T_w goal_torso = stamp_pose(mat_to_pose(tll_T_w), 'torso_lift_link') rospy.loginfo('AFTER TIP Goal is x %f y %f z %f in %s' % (goal_torso.pose.position.x, goal_torso.pose.position.y, goal_torso.pose.position.z, goal_torso.header.frame_id)) verbose = False time_ang = None min_ang_error = None time_trans = None min_trans_error = None timed_out = False while not rospy.is_shutdown(): cur_time = rospy.get_time() gripper_matrix = tfu.tf_as_matrix( self.tf.lookupTransform('torso_lift_link', self.controller_frame, rospy.Time(0))) gripper_ps = stamp_pose(mat_to_pose(gripper_matrix), 'torso_lift_link') #Someone preempted us! if self.linear_movement_as.is_preempt_requested(): #Stop our motion self.target_pub.publish( stamp_pose(gripper_ps.pose, gripper_ps.header.frame_id)) self.linear_movement_as.set_preempted() rospy.loginfo('action_cb: PREEMPTED!') break #Calc feedback if verbose: print 'current_pose %.3f %.3f %.3f, rot %.3f %.3f %.3f %.3f in %s' % ( gripper_ps.pose.position.x, gripper_ps.pose.position.y, gripper_ps.pose.position.z, gripper_ps.pose.orientation.x, gripper_ps.pose.orientation.y, gripper_ps.pose.orientation.z, gripper_ps.pose.orientation.w, gripper_ps.header.frame_id) print 'goal_pose %.3f %.3f %.3f, rot %.3f %.3f %.3f %.3f in %s' % ( goal_torso.pose.position.x, goal_torso.pose.position.y, goal_torso.pose.position.z, goal_torso.pose.orientation.x, goal_torso.pose.orientation.y, goal_torso.pose.orientation.z, goal_torso.pose.orientation.w, goal_torso.header.frame_id) trans, ang, _ = pose_distance(gripper_ps, goal_torso, self.tf) feedback = ptp.LinearMovementFeedback( gm.Vector3(trans[0, 0], trans[1, 0], trans[2, 0])) self.linear_movement_as.publish_feedback(feedback) #Reached goal trans_mag = np.linalg.norm(trans) if verbose: print trans.T, 'trans_mag', trans_mag, 'ang', abs( ang), 'rot toler', self.rot_tolerance if min_trans_error == None or min_trans_error == None: min_trans_error = trans_mag min_ang_error = abs(ang) time_ang = cur_time time_trans = cur_time if trans_mag < min_trans_error: min_trans_error = trans_mag time_trans = cur_time if abs(ang) < min_ang_error: min_ang_error = abs(ang) time_ang = cur_time if trans_tolerance > trans_mag and self.rot_tolerance > abs(ang): rospy.loginfo('action_cb: reached goal.') break #Timed out! is this a failure? if cur_time > tmax: rospy.loginfo('action_cb: timed out.') timed_out = True break #if it has been a while since we made progress if trans_mag > min_trans_error and (cur_time - time_trans) > self.stall_time: rospy.loginfo('action_cb: stalled.') break # tends to not stall on angle so don't look out for this case #if abs(ang) > min_ang_error and (cur_time - time_ang) > self.stall_time: # rospy.loginfo('action_cb: stalled.') # break #Send controls clamped_target = self.clamp_pose(goal_torso, trans_vel, rot_vel, ref_pose=gripper_ps) if verbose: print 'clamped_target', clamped_target.pose.position.x, clamped_target.pose.position.y, print clamped_target.pose.position.z, clamped_target.header.frame_id, '\n' #break self.target_pub.publish(clamped_target) #break trans, ang, _ = pose_distance(gripper_ps, goal_torso, self.tf) result = ptp.LinearMovementResult( gm.Vector3(trans[0, 0], trans[1, 0], trans[2, 0]), 'unknown') if trans_tolerance > np.linalg.norm(trans): rospy.loginfo('SUCCEEDED! %.3f ang %.3f' % (np.linalg.norm(trans), np.degrees(ang))) result.message = 'succeeded' self.linear_movement_as.set_succeeded(result) else: rospy.loginfo('ABORTED! %.3f ang %.3f' % (np.linalg.norm(trans), np.degrees(ang))) if timed_out: result.message = 'timed_out' else: result.message = 'goal_not_reached' self.linear_movement_as.set_aborted(result)
def get_pose(self): p_base = tfu.transform('map', 'base_footprint', self.tflistener) \ * tfu.tf_as_matrix(([0., 0., 0., 1.], tr.quaternion_from_euler(0,0,0))) return tfu.matrix_as_tf(p_base)
def action_cb(self, msg): rospy.loginfo('message that we got:\n' + str(msg)) self.controller_manager.cart_mode(self.arm) trans_tolerance = msg.trans_tolerance #rospy.get_param("~translation_tolerance") time_out = msg.time_out #rospy.get_param("~timeout") self.rot_tolerance = math.radians(rospy.get_param("~rotation_tolerance")) #self._wait_for_pose_message() success = False r = rospy.Rate(100) goal_ps = msg.goal #relative_movement = msg.relative trans_vel = msg.trans_vel rot_vel = msg.rot_vel if trans_vel <= 0: trans_vel = .02 if rot_vel <= 0: rot_vel = pi/20. tstart = rospy.get_time() tmax = tstart + time_out #self.controller_manager = ControllerManager() rospy.loginfo('Goal is x %f y %f z %f in %s' % (goal_ps.pose.position.x, goal_ps.pose.position.y, goal_ps.pose.position.z, goal_ps.header.frame_id)) goal_torso = change_pose_stamped_frame(self.tf, goal_ps, 'torso_lift_link') rospy.loginfo('BEFORE TIP Goal is x %f y %f z %f in %s' % (goal_torso.pose.position.x, goal_torso.pose.position.y, goal_torso.pose.position.z, goal_torso.header.frame_id)) rospy.loginfo('Tool Frame %s Controller Frame %s' % (self.tool_frame, self.controller_frame)) tll_T_tip = pose_to_mat(goal_torso.pose) tip_T_w = tf_as_matrix(self.tf.lookupTransform(self.tool_frame, self.controller_frame, rospy.Time(0))) #print 'TRANSFORM', tip_T_w tll_T_w = tll_T_tip * tip_T_w goal_torso = stamp_pose(mat_to_pose(tll_T_w), 'torso_lift_link') rospy.loginfo('AFTER TIP Goal is x %f y %f z %f in %s' % (goal_torso.pose.position.x, goal_torso.pose.position.y, goal_torso.pose.position.z, goal_torso.header.frame_id)) verbose = False time_ang = None min_ang_error = None time_trans = None min_trans_error = None timed_out = False while not rospy.is_shutdown(): cur_time = rospy.get_time() gripper_matrix = tfu.tf_as_matrix(self.tf.lookupTransform('torso_lift_link', self.controller_frame, rospy.Time(0))) gripper_ps = stamp_pose(mat_to_pose(gripper_matrix), 'torso_lift_link') #Someone preempted us! if self.linear_movement_as.is_preempt_requested(): #Stop our motion self.target_pub.publish(stamp_pose(gripper_ps.pose, gripper_ps.header.frame_id)) self.linear_movement_as.set_preempted() rospy.loginfo('action_cb: PREEMPTED!') break #Calc feedback if verbose: print 'current_pose %.3f %.3f %.3f, rot %.3f %.3f %.3f %.3f in %s' % (gripper_ps.pose.position.x, gripper_ps.pose.position.y, gripper_ps.pose.position.z, gripper_ps.pose.orientation.x, gripper_ps.pose.orientation.y, gripper_ps.pose.orientation.z, gripper_ps.pose.orientation.w, gripper_ps.header.frame_id) print 'goal_pose %.3f %.3f %.3f, rot %.3f %.3f %.3f %.3f in %s' % (goal_torso.pose.position.x, goal_torso.pose.position.y, goal_torso.pose.position.z, goal_torso.pose.orientation.x, goal_torso.pose.orientation.y, goal_torso.pose.orientation.z, goal_torso.pose.orientation.w, goal_torso.header.frame_id) trans, ang, _ = pose_distance(gripper_ps, goal_torso, self.tf) feedback = ptp.LinearMovementFeedback(gm.Vector3(trans[0,0], trans[1,0], trans[2,0])) self.linear_movement_as.publish_feedback(feedback) #Reached goal trans_mag = np.linalg.norm(trans) if verbose: print trans.T, 'trans_mag', trans_mag, 'ang', abs(ang), 'rot toler', self.rot_tolerance if min_trans_error == None or min_trans_error == None: min_trans_error = trans_mag min_ang_error = abs(ang) time_ang = cur_time time_trans = cur_time if trans_mag < min_trans_error: min_trans_error = trans_mag time_trans = cur_time if abs(ang) < min_ang_error: min_ang_error = abs(ang) time_ang = cur_time if trans_tolerance > trans_mag and self.rot_tolerance > abs(ang): rospy.loginfo('action_cb: reached goal.') break #Timed out! is this a failure? if cur_time > tmax: rospy.loginfo('action_cb: timed out.') timed_out = True break #if it has been a while since we made progress if trans_mag > min_trans_error and (cur_time - time_trans) > self.stall_time: rospy.loginfo('action_cb: stalled.') break # tends to not stall on angle so don't look out for this case #if abs(ang) > min_ang_error and (cur_time - time_ang) > self.stall_time: # rospy.loginfo('action_cb: stalled.') # break #Send controls clamped_target = self.clamp_pose(goal_torso, trans_vel, rot_vel, ref_pose=gripper_ps) if verbose: print 'clamped_target', clamped_target.pose.position.x, clamped_target.pose.position.y, print clamped_target.pose.position.z, clamped_target.header.frame_id, '\n' #break self.target_pub.publish(clamped_target) #break trans, ang, _ = pose_distance(gripper_ps, goal_torso, self.tf) result = ptp.LinearMovementResult(gm.Vector3(trans[0,0], trans[1,0], trans[2,0]), 'unknown') if trans_tolerance > np.linalg.norm(trans): rospy.loginfo( 'SUCCEEDED! %.3f ang %.3f' % (np.linalg.norm(trans), np.degrees(ang))) result.message = 'succeeded' self.linear_movement_as.set_succeeded(result) else: rospy.loginfo('ABORTED! %.3f ang %.3f' % (np.linalg.norm(trans), np.degrees(ang))) if timed_out: result.message = 'timed_out' else: result.message = 'goal_not_reached' self.linear_movement_as.set_aborted(result)