def run(self):
r_arm = 'right_arm'
l_arm = 'left_arm'
self.step()
f_r = self.xhat_force[r_arm]
f_l = self.xhat_force[l_arm]
time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
h.frame_id = 'should_not_be_using_this'
self.efri_force_r_pub.publish(FloatArray(h, f_r))
self.efri_force_l_pub.publish(FloatArray(h, f_l))
def broadcast(self):
print 'AMTIForceServer: started!'
while not rospy.is_shutdown():
time.sleep(1 / 5000.0)
self.channel.publish(
FloatArray(None,
self.force_plate.read().T.tolist()[0]))
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 set_jep(self, arm, q, duration=None):
if arm == 'right_arm':
pub = self.r_jep_cmd_pub
elif arm == 'left_arm':
pub = self.l_jep_cmd_pub
else:
raise RuntimeError('Undefined arm: %s' % (arm))
time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
pub.publish(FloatArray(h, q))
def set_impedance_scale(self, arm, s):
if arm == 'right_arm':
pub = self.r_alph_cmd_pub
elif arm == 'left_arm':
pub = self.l_alph_cmd_pub
else:
raise RuntimeError('Undefined arm: %s' % (arm))
time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
pub.publish(FloatArray(h, s))
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 __init__(self):
self.mpc_goal_pub = rospy.Publisher("/haptic_mpc/goal_pose", PoseStamped)
self.haptic_weights = rospy.Publisher("haptic_mpc/weights", HapticMpcWeights)
self.fdbk_pub = rospy.Publisher("wt_log_out", String)
self.goal_posture = rospy.Publisher("haptic_mpc/joint_trajectory", JointTrajectory, latch=True)
self.ee_pose_sub = rospy.Subscriber("/haptic_mpc/gripper_pose", PoseStamped, self.ee_pose_cb)
self.joint_state_sub = rospy.Subscriber('/l_arm_controller/state', JointTrajectoryControllerState, self.joint_state_cb)
self.ee_pose = None
self.goal_pose_sub = rospy.Subscriber("~goal_pose", PoseStamped, self.goal_cb)
self.goal_pose = None
self.last_progress_time = rospy.Time(0)
self.last_progress_err = [np.inf, np.pi/2]
self.progress_timeout = rospy.Duration(10.)
self.dist_err_thresh = 0.01
self.posture_err_thresh = 0.1
self.theta_err_thresh = np.pi/18.
self.setup_1_passed = False
self.setup_2_passed = False
self.preempted = False
#self.setup_pose = PoseStamped()
#self.setup_pose.header.frame_id = 'torso_lift_link'
#self.setup_pose.pose.position = Point(*SETUP_POSE[0])
#self.setup_pose.pose.orientation = Quaternion(*SETUP_POSE[1])
self.setup_posture = FloatArray()
self.setup_posture.data = SETUP_POSTURE
self.posture_weight = HapticMpcWeights()
self.posture_weight.position_weight = 0
self.posture_weight.orient_weight = 0
self.posture_weight.posture_weight = 1
self.orient_weight = HapticMpcWeights()
self.orient_weight.position_weight = 1
self.orient_weight.orient_weight = 1
self.orient_weight.posture_weight = 0
rospy.loginfo("[%s] Ready" %rospy.get_name())
rospy.sleep(1.0)
rospy.logout('... begin')
r_arm = 'right_arm'
l_arm = 'left_arm'
transform_bcast = tfb.TransformBroadcaster()
torso_link_name = ar.link_tf_name(r_arm, 0)
while not rospy.is_shutdown():
rospy.sleep(0.1)
f_r = arm_client.get_wrist_force(r_arm, base_frame=True)
f_l = arm_client.get_wrist_force(l_arm, base_frame=True)
time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
force_r_pub.publish(FloatArray(h, f_r))
force_l_pub.publish(FloatArray(h, f_l))
publish_sphere_marker((0.5, 0.5, 0.5), 0.08, torso_link_name,
time_stamp, 'both_arms', 0)
for arm in [r_arm, l_arm]:
q = arm_client.get_joint_angles(arm)
links = [2, 3, 7]
for i in links:
p, rot = arms.FK_all(arm, q, i)
qaut = tr.matrix_to_quaternion(rot)
frameid = ar.link_tf_name(arm, i)
transform_bcast.sendTransform(p.A1.tolist(), qaut, time_stamp,
frameid, torso_link_name)
publish_sphere_marker((0.5, 0.1, 0.5), 0.05, frameid,
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()
def step_ros(self):
r_arm = 'right_arm'
l_arm = 'left_arm'
self.cb_lock.acquire()
if self.enable_left_arm:
l_jep = copy.copy(self.l_jep)
l_alpha = copy.copy(self.arm_settings['left_arm'].stiffness_list)
if self.enable_right_arm:
r_jep = copy.copy(self.r_jep)
r_alpha = copy.copy(self.arm_settings['right_arm'].stiffness_list)
self.cb_lock.release()
if self.enable_left_arm:
self.set_jep(l_arm, l_jep)
self.set_alpha(l_arm, l_alpha)
if self.enable_right_arm:
self.set_jep(r_arm, r_jep)
self.set_alpha(r_arm, r_alpha)
self.step()
motor_pwr_state = self.is_motor_power_on()
if not motor_pwr_state:
self.maintain_configuration()
if self.enable_right_arm:
q_r = self.get_joint_angles(r_arm)
tau_r = self.get_joint_torques(r_arm)
mot_temp_r = self.get_motor_temps(r_arm)
else:
q_r = None
tau_r = None
mot_temp_r = None
if self.enable_left_arm:
q_l = self.get_joint_angles(l_arm)
tau_l = self.get_joint_torques(l_arm)
mot_temp_l = self.get_motor_temps(l_arm)
else:
q_l = None
tau_l = None
mot_temp_l = None
# it seems like this estimate of joint velocity should really be filtered
# or at least a better estimate of the derivative (higher order difference equation)
# - marc Jul 13, 2012
# added one more term to achieve a better estimate of the derivative - tapo Jan 23, 2013
t_now = rospy.get_time()
if self.q_r != None and self.q_l != None and self.q_rHOT != None and self.q_lHOT != None and self.time_stamp != None:
dt = t_now - self.time_stamp
#qdot_r = (np.array(q_r) - np.array(self.q_r)) / dt
qdot_r = (-np.array(q_r) + 4 * np.array(self.q_r) -
3 * np.array(self.q_rHOT)) / (2 * dt)
#qdot_l = (np.array(q_l) - np.array(self.q_l)) / dt
qdot_l = (-np.array(q_l) + 4 * np.array(self.q_l) -
3 * np.array(self.q_lHOT)) / (2 * dt)
else:
qdot_r = np.zeros(7)
qdot_l = np.zeros(7)
self.q_rHOT = self.q_r
self.q_lHOT = self.q_l
self.q_r = q_r
self.q_l = q_l
self.time_stamp = t_now
if self.enable_right_arm:
f_raw_r = self.get_wrist_force(r_arm).A1.tolist()
f_r = self.xhat_force[r_arm]
if self.enable_left_arm:
f_raw_l = self.get_wrist_force(l_arm).A1.tolist()
f_l = self.xhat_force[l_arm]
# publish stuff over ROS.
time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
h.frame_id = '/torso_lift_link'
pos = []
nms = []
if self.enable_right_arm:
self.q_r_pub.publish(FloatArray(h, q_r))
self.qdot_r_pub.publish(FloatArray(h, qdot_r))
self.jep_r_pub.publish(FloatArray(h, r_jep))
self.alph_r_pub.publish(FloatArray(h, r_alpha))
self.torque_r_pub.publish(FloatArray(h, tau_r))
self.mot_temp_r_pub.publish(FloatArray(h, mot_temp_r))
nms = nms + self.joint_names_list['right_arm']
pos = pos + q_r
h.frame_id = 'should_not_be_using_this'
self.force_raw_r_pub.publish(FloatArray(h, f_raw_r))
self.force_r_pub.publish(FloatArray(h, f_r))
if enable_left_arm:
self.q_l_pub.publish(FloatArray(h, q_l))
self.qdot_l_pub.publish(FloatArray(h, qdot_l))
self.jep_l_pub.publish(FloatArray(h, l_jep))
self.alph_l_pub.publish(FloatArray(h, l_alpha))
self.torque_l_pub.publish(FloatArray(h, tau_l))
self.mot_temp_l_pub.publish(FloatArray(h, mot_temp_l))
nms = nms + self.joint_names_list['left_arm']
pos = pos + q_l
h.frame_id = 'should_not_be_using_this'
self.force_raw_l_pub.publish(FloatArray(h, f_raw_l))
self.force_l_pub.publish(FloatArray(h, f_l))
self.joint_state_pub.publish(
JointState(h, nms, pos, [0.] * len(pos), [0.] * len(pos)))
self.pwr_state_pub.publish(Bool(motor_pwr_state))
data, tme = msg
ftvalue = ftc.binary_to_ft(data)
ftvalue = np.array(ftvalue)
for i in range(3):
xhat, p = kalman_update(xhat_force[i], P_force[i], 1e-3, 0.04,
ftvalue[i])
P_force[i] = p
xhat_force[i] = xhat
#ftvalue[i] = xhat
xhat_force[3] = ftvalue[3]
xhat_force[4] = ftvalue[4]
xhat_force[5] = ftvalue[5]
ftvalue = ftvalue.tolist()
channel.publish(
FloatArray(rospy.Header(stamp=rospy.Time.from_seconds(tme)),
xhat_force))
channel2.publish(
FloatArray(rospy.Header(stamp=rospy.Time.from_seconds(tme)),
ftvalue))
chan_vec3.publish(FTread_to_Force(ftvalue, opt.name))
#times.append(time.time())
#else:
# time.sleep(1/5000.0)
time.sleep(1 / 5000.0)
#import pylab as pl
#import numpy as np
#a = np.array(times)
#pl.plot(a[1:] - a[:-1], '.')
#pl.show()
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 )
def step_ros(self):
r_arm = 'right_arm'
l_arm = 'left_arm'
self.cb_lock.acquire()
r_jep = copy.copy(self.r_jep)
l_jep = copy.copy(self.l_jep)
r_alpha = copy.copy(self.arm_settings['right_arm'].stiffness_list)
l_alpha = copy.copy(self.arm_settings['left_arm'].stiffness_list)
self.cb_lock.release()
self.set_jep(r_arm, r_jep)
self.set_jep(l_arm, l_jep)
self.set_alpha(r_arm, r_alpha)
self.set_alpha(l_arm, l_alpha)
self.step()
motor_pwr_state = self.is_motor_power_on()
if not motor_pwr_state:
self.maintain_configuration()
q_r = self.get_joint_angles(r_arm)
q_l = self.get_joint_angles(l_arm)
if self.floating_arms:
if self.qr_prev == None or self.floating_arms_counter < 100:
self.qr_prev = q_r
self.ql_prev = q_l
self.floating_arms_counter += 1
else:
tol = np.radians([5., 2., 10., 2., 10., 0.03, 0.6])
r_arr = np.array(q_r)
l_arr = np.array(q_l)
prev_r_arr = np.array(self.qr_prev)
prev_l_arr = np.array(self.ql_prev)
dqr = np.array(q_r) - np.array(prev_r_arr)
dql = np.array(q_l) - np.array(prev_l_arr)
dqr = dqr * (np.abs(dqr) > tol)
dql = dql * (np.abs(dql) > tol)
r_jep = (np.array(r_jep) + dqr).tolist()
l_jep = (np.array(l_jep) + dql).tolist()
self.cb_lock.acquire()
self.r_jep = copy.copy(r_jep)
self.l_jep = copy.copy(l_jep)
self.cb_lock.release()
change_idxs = np.where(dqr != 0)
prev_r_arr[change_idxs] = r_arr[change_idxs]
change_idxs = np.where(dql != 0)
prev_l_arr[change_idxs] = l_arr[change_idxs]
self.qr_prev = prev_r_arr.tolist()
self.ql_prev = prev_l_arr.tolist()
f_raw_r = self.get_wrist_force(r_arm).A1.tolist()
f_raw_l = self.get_wrist_force(l_arm).A1.tolist()
f_r = self.xhat_force[r_arm]
f_l = self.xhat_force[l_arm]
# publish stuff over ROS.
time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
self.q_r_pub.publish(FloatArray(h, q_r))
self.q_l_pub.publish(FloatArray(h, q_l))
self.jep_r_pub.publish(FloatArray(h, r_jep))
self.jep_l_pub.publish(FloatArray(h, l_jep))
self.alph_r_pub.publish(FloatArray(h, r_alpha))
self.alph_l_pub.publish(FloatArray(h, l_alpha))
h.frame_id = '/torso_lift_link'
nms = self.joint_names_list['right_arm'] + self.joint_names_list[
'left_arm']
pos = q_r + q_l
self.joint_state_pub.publish(
JointState(h, nms, pos, [0.] * len(pos), [0.] * len(pos)))
h.frame_id = ar.link_tf_name(r_arm, 7)
self.force_raw_r_pub.publish(FloatArray(h, f_raw_r))
self.force_r_pub.publish(FloatArray(h, f_r))
h.frame_id = ar.link_tf_name(l_arm, 7)
self.force_raw_l_pub.publish(FloatArray(h, f_raw_l))
self.force_l_pub.publish(FloatArray(h, f_l))
self.pwr_state_pub.publish(Bool(motor_pwr_state))
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)
def step_ros(self):
r_arm = 'right_arm'
l_arm = 'left_arm'
self.cb_lock.acquire()
r_jep = copy.copy(self.r_jep)
l_jep = copy.copy(self.l_jep)
r_alpha = copy.copy(self.arm_settings['right_arm'].stiffness_list)
l_alpha = copy.copy(self.arm_settings['left_arm'].stiffness_list)
self.cb_lock.release()
self.set_jep(r_arm, r_jep)
self.set_jep(l_arm, l_jep)
self.set_alpha(r_arm, r_alpha)
self.set_alpha(l_arm, l_alpha)
self.step()
motor_pwr_state = self.is_motor_power_on()
if not motor_pwr_state:
self.maintain_configuration()
q_r = self.get_joint_angles(r_arm)
q_l = self.get_joint_angles(l_arm)
t_now = rospy.get_time()
if self.q_r != None and self.q_l != None and self.time_stamp != None:
dt = t_now - self.time_stamp
qdot_r = (np.array(q_r) - np.array(self.q_r)) / dt
qdot_l = (np.array(q_l) - np.array(self.q_l)) / dt
else:
qdot_r = np.zeros(7)
qdot_l = np.zeros(7)
self.q_r = q_r
self.q_l = q_l
self.time_stamp = t_now
f_raw_r = self.get_wrist_force(r_arm).A1.tolist()
f_raw_l = self.get_wrist_force(l_arm).A1.tolist()
f_r = self.xhat_force[r_arm]
f_l = self.xhat_force[l_arm]
# publish stuff over ROS.
time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
self.q_r_pub.publish(FloatArray(h, q_r))
self.q_l_pub.publish(FloatArray(h, q_l))
self.qdot_r_pub.publish(FloatArray(h, qdot_r))
self.qdot_l_pub.publish(FloatArray(h, qdot_l))
self.jep_r_pub.publish(FloatArray(h, r_jep))
self.jep_l_pub.publish(FloatArray(h, l_jep))
self.alph_r_pub.publish(FloatArray(h, r_alpha))
self.alph_l_pub.publish(FloatArray(h, l_alpha))
h.frame_id = '/torso_lift_link'
nms = self.joint_names_list['right_arm'] + self.joint_names_list['left_arm']
pos = q_r + q_l
self.joint_state_pub.publish(JointState(h, nms, pos,
[0.]*len(pos), [0.]*len(pos)))
h.frame_id = 'should_not_be_using_this'
self.force_raw_r_pub.publish(FloatArray(h, f_raw_r))
self.force_r_pub.publish(FloatArray(h, f_r))
h.frame_id = 'should_not_be_using_this'
self.force_raw_l_pub.publish(FloatArray(h, f_raw_l))
self.force_l_pub.publish(FloatArray(h, f_l))
self.pwr_state_pub.publish(Bool(motor_pwr_state))
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__':
import sys
rospy.init_node('ati_ft_emulator')
print sys.argv
# 4 for roslaunch
if len(sys.argv) != 2 and len(sys.argv) != 4:
rospy.logerr(
'Need to pass the topic name on the command line. Exiting...')
sys.exit()
topic = sys.argv[1]
lock = RLock()
ft_val = [0.] * 6
pub = rospy.Subscriber('/r_cart/state/wrench', Twist, ft_cb)
pub = rospy.Publisher(topic, FloatArray)
rospy.loginfo('Started the ATI FT emulator.')
rt = rospy.Rate(100)
while not rospy.is_shutdown():
lock.acquire()
send_ft_val = copy.copy(ft_val)
lock.release()
fa = FloatArray(rospy.Header(stamp=rospy.Time.now()), send_ft_val)
pub.publish(fa)
rt.sleep()
def set_ep(self, jep, duration=None):
time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
self.jep_cmd_pub.publish(FloatArray(h, jep))
self.publish_rviz_markers()
def set_impedance_scale(self, s):
time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
self.alph_cmd_pub.publish(FloatArray(h, s))
def run(self):
print 'PoseBroadcast: started.'
while self.should_run and not rospy.is_shutdown():
sleep(1.0 / self.frequency)
self.pose = self.zenither.get_position_meters()
self.channel.publish(FloatArray(None, [self.pose]))
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()
