def publish_skin_contact(self, ft):
# -ve sign because we want the force that the robot is applying
# on the world.
ft = -np.matrix(ft).T
force = ft[0:3, :]
f_mag = np.linalg.norm(force)
torque = ft[3:, :]
msg = SkinContact()
if self.use_right_arm:
frm_id = '/handmount_RIGHT'
else:
frm_id = '/handmount_LEFT'
msg.header.frame_id = '/torso_lift_link'
msg.header.stamp = rospy.Time.now()
if f_mag > 2.0:
t, q = self.tf_lstnr.lookupTransform('/torso_lift_link', frm_id,
rospy.Time(0))
t = np.matrix(t).reshape(3, 1)
rot = tr.quaternion_to_matrix(q)
# approx location of the FT sensor in the handmount_LEFT
# or handmount_RIGHT frame.
pt = np.matrix([0., 0., -0.06]).T
# using a fixed location for the contact, for now.
if False:
# here I can try and compute the line of action of
# the force and then attempt to find the contact
# location by taking the intersection of the line of
# action with the surface of the cylinder.
# Too complicated for now (Sept 29, 2011)
p_oc = np.cross(force.A1, torque.A1) / (force.T * force)[0, 0]
pt = np.matrix(p_oc).T + pt
# now transform vectors into the torso_lift_link
force = rot * force
n = force / f_mag
pt = rot * pt + t
msg.locations.append(Point(pt[0, 0], pt[1, 0], pt[2, 0]))
msg.forces.append(Vector3(force[0, 0], force[1, 0], force[2, 0]))
msg.normals.append(Vector3(n[0, 0], n[1, 0], n[2, 0]))
msg.pts_x.append(FloatArrayBare([pt[0, 0]]))
msg.pts_y.append(FloatArrayBare([pt[1, 0]]))
msg.pts_z.append(FloatArrayBare([pt[2, 0]]))
if self.use_right_arm:
msg.link_names.append('end_effector_RIGHT')
else:
msg.link_names.append('end_effector_LEFT')
self.sc_pub.publish(msg)
def taxel_array_cb(ta, callback_args):
sc_pu, tf_lstnr = callback_args
sc = SkinContact()
sc.header.frame_id = '/torso_lift_link' # has to be this and no other coord frame.
sc.header.stamp = ta.header.stamp
pts = np.column_stack((ta.centers_x, ta.centers_y, ta.centers_z))
nrmls = np.column_stack((ta.normals_x, ta.normals_y, ta.normals_z))
fs = np.column_stack((ta.values_x, ta.values_y, ta.values_z))
t1, q1 = tf_lstnr.lookupTransform(sc.header.frame_id, ta.header.frame_id,
rospy.Time(0))
#ta.header.stamp)
t1 = np.matrix(t1).reshape(3, 1)
r1 = tr.quaternion_to_matrix(q1)
if ta.link_names == []:
real_skin_patch = True
else:
real_skin_patch = False
# transform to the torso_lift_link frame.
pts = r1 * np.matrix(pts).T + t1
nrmls = r1 * np.matrix(nrmls).T
fs = r1 * np.matrix(fs).T
fmags = ut.norm(fs).A1
# for fabric sensor and meka sensor, Advait moved the thresholding
# etc within the calibration node. (June 13, 2012)
if not real_skin_patch:
idxs = np.where(fmags > 0.5)[0]
else:
idxs = np.where(fmags > 0.001)[0]
for i in idxs:
p = pts[:, i]
n1 = nrmls[:, i]
n2 = fs[:, i]
if real_skin_patch:
link_name = ta.header.frame_id
else:
link_name = ta.link_names[i]
sc.locations.append(Point(p[0, 0], p[1, 0], p[2, 0]))
sc.forces.append(Vector3(n2[0, 0], n2[1, 0], n2[2, 0]))
sc.normals.append(Vector3(n1[0, 0], n1[1, 0], n1[2, 0]))
sc.link_names.append(link_name)
sc.pts_x.append(FloatArrayBare(p[0, :].A1))
sc.pts_y.append(FloatArrayBare(p[1, :].A1))
sc.pts_z.append(FloatArrayBare(p[2, :].A1))
sc_pub.publish(sc)
def set_delta_ep_ros(self, msg):
delta_jep = copy.copy(msg.data)
if delta_jep is None or len(delta_jep) != 3:
raise RuntimeError("set_jep value is " + str(delta_jep))
with self.lock:
if self.ep == None:
self.ep = self.get_joint_angles()
jep = (np.array(self.ep) + np.array(delta_jep)).tolist()
f = FloatArrayBare(jep)
self.jep_pub.publish(f)
def set_delta_ep_ros(self, msg, duration=0.15):
delta_jep = copy.copy(msg.data)
if delta_jep is None or len(delta_jep) != len(self.q):
raise RuntimeError("set_jep value is " + str(delta_jep))
with self.lock:
if self.ep == None:
self.ep = self.get_joint_angles()
#jep = (np.array(self.ep) + np.array(delta_jep)).tolist()
self.ep = (np.array(self.ep) + np.array(delta_jep)).tolist()
#f = FloatArrayBare(jep)
f = FloatArrayBare(self.ep)
self.jep_pub.publish(f)
self.publish_rviz_markers()
def skin_contact_cb(sc, callback_args):
sc_pub, kinematics = callback_args
sf = SkinContact() # sf - single force
sf.header.frame_id = '/torso_lift_link' # has to be this and no other coord frame.
sf.header.stamp = sc.header.stamp
if len(sc.link_names) == 0:
sc_pub.publish(sc)
return
fc_l = []
xc_l = []
# no guarantee that contacts for a particular link will be
# together in the list. so, i am sorting the list and preserving
# the indices to get the appropriate forces and locations.
link_names = sc.link_names
idxs = range(len(link_names))
sorted_list = zip(link_names, idxs)
sorted_list.sort()
i = 0
for nm, idx in sorted_list:
jt_num = int(nm[-1]) - 1 # assume that links are named link1 ...
fc_l.append(
np.matrix([sc.forces[idx].x, sc.forces[idx].y,
sc.forces[idx].z]).T)
xc_l.append(
np.matrix([
sc.locations[idx].x, sc.locations[idx].y, sc.locations[idx].z
]).T)
if i == len(sorted_list) - 1 or sorted_list[i + 1][0] != nm:
o, _ = kinematics.FK(q, jt_num)
o_next, _ = kinematics.FK(q, jt_num + 1)
f_res, loc = simulate_ft_sensor_on_link_base(
np.column_stack(fc_l), np.column_stack(xc_l), o, o_next)
if loc == None:
continue
nrml = np.cross(np.array([0., 0., 1.]), (o_next - o).A1)
nrml = np.matrix(nrml / np.linalg.norm(nrml)).T
if (nrml.T * f_res)[0, 0] < 0.:
nrml = -1. * nrml
f_res_direc = f_res / np.linalg.norm(f_res)
link_length = np.linalg.norm(o - o_next)
link_direc = (o_next - o) / link_length
if (link_direc.T * f_res_direc)[0, 0] > 0.93:
# here angle b/w nrml and f_res is < 20 degrees
loc = o_next
elif (link_direc.T * f_res_direc)[0, 0] < -0.93:
f_res = f_res * -1
loc = o_next
f_res_direc = -f_res_direc
dist_from_o = np.linalg.norm(loc - o)
dist_from_o_next = np.linalg.norm(loc - o_next)
if dist_from_o > link_length or dist_from_o_next > link_length:
if dist_from_o > dist_from_o_next:
loc = o_next
else:
loc = o
# contact close to the tip, assume circular tip and so
# contact normal can be anything.
if np.linalg.norm(loc - o_next) < 0.02:
nrml = f_res_direc
sf.link_names.append(nm)
sf.forces.append(Vector3(f_res[0, 0], f_res[1, 0], f_res[2, 0]))
sf.locations.append(Point(loc[0, 0], loc[1, 0], loc[2, 0]))
sf.normals.append(Vector3(nrml[0, 0], nrml[1, 0], nrml[2, 0]))
sf.pts_x.append(FloatArrayBare([loc[0, 0]]))
sf.pts_y.append(FloatArrayBare([loc[1, 0]]))
sf.pts_z.append(FloatArrayBare([loc[2, 0]]))
fc_l = []
xc_l = []
i += 1
sc_pub.publish(sf)
def set_ep(self, jep, duration=0.15):
f = FloatArrayBare(jep)
self.jep_pub.publish(f)
self.publish_rviz_markers()
def set_ep_ros(self, msg, duration=0.15):
f = FloatArrayBare(msg.data)
self.jep_pub.publish(f)
self.publish_rviz_markers()
def publish_to_autobed(configuration_goals_list):
autobed_pub = rospy.Publisher('/abdin0', FloatArrayBare, latch=True)
autobed_goal = FloatArrayBare()
autobed_goal.data = [configuration_goals_list[0][2], configuration_goals_list[0][1], 0.]
autobed_pub.publish(autobed_goal)