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()
