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)