def generateMechData(self): # Estimate door angle and tangential force. mech_data = None ## add something [ee_pos, ee_quat] = self.getEndeffectorPose() if self.aPts is None: self.aPts = ee_pos pts_2d = self.aPts[:2, :] else: self.aPts = np.hstack([self.aPts, ee_pos]) pts_2d = self.aPts[:2, :] x_guess = self.aPts[0, 0] y_guess = self.aPts[1, 0] - self.radius rad_guess = self.radius rad, cx, cy = at.fit_circle(rad_guess, x_guess, y_guess, pts_2d, method='fmin_bfgs', verbose=False, rad_fix=True) ## print 'rad, cx, cy:', rad, cx, cy p0 = self.aPts[:, 0] # 3x1 rad_vec_init = np.matrix((p0[0, 0] - cx, p0[1, 0] - cy)).T rad_vec_init = rad_vec_init / np.linalg.norm(rad_vec_init) rad_vec = np.array([ee_pos[0, 0] - cx, ee_pos[1, 0] - cy]) rad_vec = rad_vec / np.linalg.norm(rad_vec) ang = np.arccos((rad_vec.T * rad_vec_init)[0, 0]) tan_vec = (np.matrix([[0, -1], [1, 0]]) * np.matrix(rad_vec).T).A1 f_vec = np.array([self.ft_data[0], self.ft_data[1]]) f_tan_mag = abs(np.dot(f_vec, tan_vec)) msg = FloatArray() msg.data = [f_tan_mag, ang] self.mech_data_pub.publish(msg)
def generateMechData(self): # Estimate door angle and tangential force. mech_data = None ## add something [ee_pos, ee_quat] = self.getEndeffectorPose() if self.aPts is None: self.aPts = ee_pos pts_2d = self.aPts[:2,:] else: self.aPts = np.hstack([self.aPts, ee_pos]) pts_2d = self.aPts[:2,:] x_guess = self.aPts[0,0] y_guess = self.aPts[1,0] - self.radius rad_guess = self.radius rad, cx, cy = at.fit_circle(rad_guess,x_guess,y_guess,pts_2d, method='fmin_bfgs',verbose=False, rad_fix=True) ## print 'rad, cx, cy:', rad, cx, cy p0 = self.aPts[:,0] # 3x1 rad_vec_init = np.matrix((p0[0,0]-cx, p0[1,0]-cy)).T rad_vec_init = rad_vec_init / np.linalg.norm(rad_vec_init) rad_vec = np.array([ee_pos[0,0]-cx,ee_pos[1,0]-cy]) rad_vec = rad_vec/np.linalg.norm(rad_vec) ang = np.arccos((rad_vec.T*rad_vec_init)[0,0]) tan_vec = (np.matrix([[0,-1],[1,0]]) * np.matrix(rad_vec).T).A1 f_vec = np.array([self.ft_data[0],self.ft_data[1]]) f_tan_mag = abs(np.dot(f_vec, tan_vec)) msg = FloatArray() msg.data = [f_tan_mag, ang] self.mech_data_pub.publish(msg)
import roslib; roslib.load_manifest('hrl_lib') import rospy import std_srvs.srv as srv from hrl_msgs.msg import FloatArray import time x = 3.0 rospy.init_node('trial_publisher') pub = rospy.Publisher( 'trial_pub', FloatArray) while not rospy.is_shutdown(): x += 0.1 fa = FloatArray() fa.data = [ x, x+1.0, x+2.0 ] pub.publish( fa ) time.sleep( 0.3 )
import roslib roslib.load_manifest('hrl_lib') import rospy import std_srvs.srv as srv from hrl_msgs.msg import FloatArray import time x = 3.0 rospy.init_node('trial_publisher') pub = rospy.Publisher('trial_pub', FloatArray) while not rospy.is_shutdown(): x += 0.1 fa = FloatArray() fa.data = [x, x + 1.0, x + 2.0] pub.publish(fa) time.sleep(0.3)
l = get_adc_data(serial_dev, num_adc_inputs) return l if __name__ == '__main__': dev_name = '/dev/ttyUSB4' #dev_name = '/dev/ttyACM0' #dev_name = '/dev/robot/arduino1' #dev_name = '/dev/robot/arduino2' baudrate = 115200 serial_dev = setup_serial(dev_name, baudrate) pub = rospy.Publisher('/arduino/ADC', FloatArray) rospy.init_node('adc_publisher_node') for i in range(10): ln = serial_dev.readline() rospy.loginfo('Started publishing ADC data') while not rospy.is_shutdown(): fa = FloatArray() fa.header.stamp = rospy.Time.now() fa.data = get_adc_data(serial_dev, 6) pub.publish(fa) serial_dev.close()
from hrl_msgs.msg import FloatArray if __name__ == '__main__': ft_client = ftc.FTClient('force_torque_ft2') ft_client.bias() marker_pub = rospy.Publisher('/test_marker', Marker) ati_pub = rospy.Publisher('/ati_ft', FloatArray) p_st = np.matrix([0.,0.,0.]).T force_frame_id = 'r_wrist_roll_link' while not rospy.is_shutdown(): ft = ft_client.read(fresh = True) rmat = tr.Rx(math.radians(180.)) * tr.Ry(math.radians(-90.)) * tr.Rz(math.radians(30.)) force = rmat * ft[0:3,:] print 'Force:', force.A1 # force is now in the 'robot' coordinate frame. force_scale = 0.1 p_end = p_st + force * force_scale marker = get_marker_arrow(p_st, p_end, force_frame_id) marker_pub.publish(marker) farr = FloatArray() farr.header.stamp = rospy.rostime.get_rostime() farr.header.frame_id = force_frame_id farr.data = (-force).A1.tolist() ati_pub.publish(farr) # rospy.sleep(0.1)
if __name__ == '__main__': dev_name = '/dev/ttyUSB4' #dev_name = '/dev/ttyACM0' #dev_name = '/dev/robot/arduino1' #dev_name = '/dev/robot/arduino2' baudrate = 115200 serial_dev = setup_serial(dev_name, baudrate) pub = rospy.Publisher('/arduino/ADC', FloatArray) rospy.init_node('adc_publisher_node') for i in range(10): ln = serial_dev.readline() rospy.loginfo('Started publishing ADC data') while not rospy.is_shutdown(): fa = FloatArray() fa.header.stamp = rospy.Time.now() fa.data = get_adc_data(serial_dev, 6) pub.publish(fa) serial_dev.close()