def r_cart_state_cb(self, msg): try: trans, quat = self.tf_lstnr.lookupTransform('/torso_lift_link', 'r_gripper_tool_frame', rospy.Time(0)) rot = tr.quaternion_to_matrix(quat) tip = np.matrix([0.12, 0., 0.]).T self.r_ee_pos = rot*tip + np.matrix(trans).T self.r_ee_rot = rot marker = Marker() marker.header.frame_id = 'torso_lift_link' time_stamp = rospy.Time.now() marker.header.stamp = time_stamp marker.ns = 'aloha land' marker.type = Marker.SPHERE marker.action = Marker.ADD marker.pose.position.x = self.r_ee_pos[0,0] marker.pose.position.y = self.r_ee_pos[1,0] marker.pose.position.z = self.r_ee_pos[2,0] size = 0.02 marker.scale.x = size marker.scale.y = size marker.scale.z = size marker.lifetime = rospy.Duration() marker.id = 71 marker.pose.orientation.x = 0 marker.pose.orientation.y = 0 marker.pose.orientation.z = 0 marker.pose.orientation.w = 1 color = (0.5, 0., 0.0) marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.color.a = 1. self.marker_pub.publish(marker) ros_pt = msg.x_desi_filtered.pose.position x, y, z = ros_pt.x, ros_pt.y, ros_pt.z self.r_cep_pos = np.matrix([x, y, z]).T pt = rot.T * (np.matrix([x,y,z]).T - np.matrix(trans).T) pt = pt + tip self.r_cep_pos_hooktip = rot*pt + np.matrix(trans).T ros_quat = msg.x_desi_filtered.pose.orientation quat = (ros_quat.x, ros_quat.y, ros_quat.z, ros_quat.w) self.r_cep_rot = tr.quaternion_to_matrix(quat) except: return
def r_cart_state_cb(self, msg): trans, quat = self.tf_lstnr.lookupTransform('/torso_lift_link', 'r_gripper_tool_frame', rospy.Time(0)) rot = tr.quaternion_to_matrix(quat) tip = np.matrix([0.12, 0., 0.]).T self.r_ee_pos = rot*tip + np.matrix(trans).T self.r_ee_rot = rot marker = Marker() marker.header.frame_id = 'torso_lift_link' time_stamp = rospy.Time.now() marker.header.stamp = time_stamp marker.ns = 'aloha land' marker.type = Marker.SPHERE marker.action = Marker.ADD marker.pose.position.x = self.r_ee_pos[0,0] marker.pose.position.y = self.r_ee_pos[1,0] marker.pose.position.z = self.r_ee_pos[2,0] size = 0.02 marker.scale.x = size marker.scale.y = size marker.scale.z = size marker.lifetime = rospy.Duration() marker.id = 71 marker.pose.orientation.x = 0 marker.pose.orientation.y = 0 marker.pose.orientation.z = 0 marker.pose.orientation.w = 1 color = (0.5, 0., 0.0) marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.color.a = 1. self.marker_pub.publish(marker) ros_pt = msg.x_desi_filtered.pose.position x, y, z = ros_pt.x, ros_pt.y, ros_pt.z self.r_cep_pos = np.matrix([x, y, z]).T pt = rot.T * (np.matrix([x,y,z]).T - np.matrix(trans).T) pt = pt + tip self.r_cep_pos_hooktip = rot*pt + np.matrix(trans).T ros_quat = msg.x_desi_filtered.pose.orientation quat = (ros_quat.x, ros_quat.y, ros_quat.z, ros_quat.w) self.r_cep_rot = tr.quaternion_to_matrix(quat)
def FK(self, arm, q): fk_req = GetPositionFKRequest() fk_req.header.frame_id = 'torso_lift_link' if arm == 0: fk_req.fk_link_names.append('r_wrist_roll_link') else: fk_req.fk_link_names.append('l_wrist_roll_link') fk_req.robot_state.joint_state.name = self.joint_names_list[arm] fk_req.robot_state.joint_state.position = q fk_resp = self.fk_srv[arm].call(fk_req) if fk_resp.error_code.val == fk_resp.error_code.SUCCESS: x = fk_resp.pose_stamped[0].pose.position.x y = fk_resp.pose_stamped[0].pose.position.y z = fk_resp.pose_stamped[0].pose.position.z pos = [x, y, z] q1 = fk_resp.pose_stamped[0].pose.orientation.x q2 = fk_resp.pose_stamped[0].pose.orientation.y q3 = fk_resp.pose_stamped[0].pose.orientation.z q4 = fk_resp.pose_stamped[0].pose.orientation.w quat = [q1,q2,q3,q4] rot = tr.quaternion_to_matrix(quat) else: rospy.logerr('Forward kinematics failed') return None, None return pos, rot
def r_cart_state_cb(self, msg): trans, quat = self.tf_lstnr.lookupTransform('/torso_lift_link', 'r_gripper_tool_frame', rospy.Time(0)) rot = tr.quaternion_to_matrix(quat) tip = np.matrix([0.12, 0., 0.]).T self.r_ee_pos = rot*tip + np.matrix(trans).T self.r_ee_rot = rot ros_pt = msg.x_desi_filtered.pose.position x, y, z = ros_pt.x, ros_pt.y, ros_pt.z self.r_cep_pos = np.matrix([x, y, z]).T pt = rot.T * (np.matrix([x,y,z]).T - np.matrix(trans).T) pt = pt + tip self.r_cep_pos_hooktip = rot*pt + np.matrix(trans).T ros_quat = msg.x_desi_filtered.pose.orientation quat = (ros_quat.x, ros_quat.y, ros_quat.z, ros_quat.w) self.r_cep_rot = tr.quaternion_to_matrix(quat)
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 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 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 get_forces(self, bias = True): # later I might be looping over all the different objects, # returning a dictionary of <object_id: force_vector> f = self.obj1_ftc.read(without_bias = not bias) f = f[0:3, :] trans, quat = self.tf_lstnr.lookupTransform('/torso_lift_link', '/ft2', rospy.Time(0)) rot = tr.quaternion_to_matrix(quat) f = rot * f return -f # the negative is intentional (Advait, Nov 24. 2010.)
def precompute_taxel_location_and_normal(self): resp = self.local_coord_frames_srv() pos_list = [] nrml_list = [] for t in resp.data: pos = (t.translation.x, t.translation.y, t.translation.z) pos_list.append(pos) q = (t.rotation.x, t.rotation.y, t.rotation.z, t.rotation.w) rot_mat = tr.quaternion_to_matrix(q) nrml_list.append(rot_mat[:, 2].A1) self.pos_arr = np.array(pos_list) self.nrml_arr = np.array(nrml_list)
def get_wrist_force(self, arm, bias=True, base_frame=False): if arm != 0: rospy.logerr('Unsupported arm: %d' % arm) raise RuntimeError('Unimplemented function') f = self.r_arm_ftc.read(without_bias=not bias) f = f[0:3, :] if base_frame: trans, quat = self.tf_lstnr.lookupTransform( '/torso_lift_link', '/ft2', rospy.Time(0)) rot = tr.quaternion_to_matrix(quat) f = rot * f return -f # the negative is intentional (Advait, Nov 24. 2010.)
def precompute_taxel_location_and_normal(self): resp = self.local_coord_frames_srv() pos_list = [] nrml_list = [] for t in resp.data: pos = (t.translation.x, t.translation.y, t.translation.z) pos_list.append(pos) q = (t.rotation.x, t.rotation.y, t.rotation.z, t.rotation.w) rot_mat = tr.quaternion_to_matrix(q) nrml_list.append(rot_mat[:,2].A1) self.pos_arr = np.array(pos_list) self.nrml_arr = np.array(nrml_list)
def get_wrist_force(self, arm, bias = True, base_frame = False): if arm != 0: rospy.logerr('Unsupported arm: %d'%arm) raise RuntimeError('Unimplemented function') f = self.r_arm_ftc.read(without_bias = not bias) f = f[0:3, :] if base_frame: trans, quat = self.tf_lstnr.lookupTransform('/torso_lift_link', '/ft2', rospy.Time(0)) rot = tr.quaternion_to_matrix(quat) f = rot * f return -f # the negative is intentional (Advait, Nov 24. 2010.)
def pose_cb(data, d): if len(data.objects) != 2: return for obj in data.objects: p = obj.pose.position pos = np.matrix([p.x, p.y, p.z]).T q = obj.pose.orientation rot = tr.quaternion_to_matrix([q.x, q.y, q.z, q.w]) t = data.header.stamp.to_sec() d[obj.type]['pos'] = pos d[obj.type]['rot'] = rot
def make_darci_ee_marker(ps, color, orientation=None, marker_array=None, control=None, mesh_id_start=0, ns="darci_ee", offset=-0.14): mesh_id = mesh_id_start # this is the palm piece mesh = Marker() mesh.ns = ns #mesh.mesh_use_embedded_materials = True mesh.scale = Vector3(1., 1., 1.) mesh.color = ColorRGBA(*color) # stub_end_effector.dae # stub_end_effector_mini45.dae # tube_with_ati_collisions.dae # wedge_blender.dae # mesh.mesh_resource = "package://hrl_dynamic_mpc/meshes/tube_with_ati_collisions.dae" mesh.mesh_resource = "package://hrl_dynamic_mpc/meshes/Darci_Flipper.stl" mesh.type = Marker.MESH_RESOURCE #rot_default = tr.Ry(np.radians(-90))*tr.Rz(np.radians(90)) rot_default = tr.Ry(np.radians(0)) * tr.Rz(np.radians(90)) if orientation == None: orientation = [0, 0, 0, 1] rot_buf = tr.quaternion_to_matrix(orientation) orientation = tr.matrix_to_quaternion(rot_buf * rot_default) mesh.pose.orientation = Quaternion(*orientation) mesh.pose.position.x = offset if control != None: control.markers.append(mesh) elif marker_array != None: mesh.pose.position = ps.pose.position mesh.header.frame_id = ps.header.frame_id mesh.id = mesh_id marker_array.markers.append(mesh) if control != None: control.interaction_mode = InteractiveMarkerControl.BUTTON return control elif marker_array != None: return marker_array
def transformTaxelArray(self, ta_msg, new_frame): # Get the transformation from the desired frame to current frame if ta_msg.header.frame_id == "": return ta_msg self.tf_lstnr.waitForTransform(new_frame, ta_msg.header.frame_id, rospy.Time(0), rospy.Duration(4.0)) t1, q1 = self.tf_lstnr.lookupTransform(new_frame, ta_msg.header.frame_id, rospy.Time(0)) t1 = np.matrix(t1).reshape(3, 1) r1 = tr.quaternion_to_matrix(q1) # Create new message data structure new_ta_msg = copy.copy(ta_msg) new_ta_msg.header.frame_id = new_frame # Perform the transformation pts = np.column_stack( (ta_msg.centers_x, ta_msg.centers_y, ta_msg.centers_z)) nrmls = np.column_stack( (ta_msg.normals_x, ta_msg.normals_y, ta_msg.normals_z)) values = np.column_stack( (ta_msg.values_x, ta_msg.values_y, ta_msg.values_z)) pts = r1 * np.matrix(pts).T + t1 nrmls = r1 * np.matrix(nrmls).T values = r1 * np.matrix(values).T # Reformat the transformed data to be repackaged as a TaxelArray message pts_array = np.asarray(pts) nrmls_array = np.asarray(nrmls) values_array = np.asarray(values) new_ta_msg.centers_x = pts_array[0, :].tolist() new_ta_msg.centers_y = pts_array[1, :].tolist() new_ta_msg.centers_z = pts_array[2, :].tolist() new_ta_msg.normals_x = nrmls_array[0, :].tolist() new_ta_msg.normals_y = nrmls_array[1, :].tolist() new_ta_msg.normals_z = nrmls_array[2, :].tolist() new_ta_msg.values_x = values_array[0, :].tolist() new_ta_msg.values_y = values_array[1, :].tolist() new_ta_msg.values_z = values_array[2, :].tolist() return new_ta_msg
def detect_artag(self): try: rospy.logout("Finding the AR tag..") self.pub_rate.sleep() (self.ar_pos, rot) = self.tf_listener.lookupTransform("/torso_lift_link", self.marker_frame, rospy.Time(0)) self.pub_rate.sleep() gripper_rot = hrl_tr.rotY(math.pi/2) #gripper facing -z direction self.ar_rot = hrl_tr.quaternion_to_matrix(rot)*gripper_rot rospy.logout("Found AR tag!\nPosition: "+pplist(self.ar_pos)+"\nQuaterion: "+pplist(rot)) self.ar_ep = [] self.ar_ep.append(np.matrix(self.ar_pos).T) self.ar_ep.append(self.ar_rot) self.found_tag = True except: rospy.logout('AARtagDetect: Transform failed for '+self.tool) return False
def make_darci_ee_marker(ps, color, orientation = None, marker_array=None, control=None, mesh_id_start = 0, ns = "darci_ee", offset=-0.14): mesh_id = mesh_id_start # this is the palm piece mesh = Marker() mesh.ns = ns #mesh.mesh_use_embedded_materials = True mesh.scale = Vector3(1.,1.,1.) mesh.color = ColorRGBA(*color) # stub_end_effector.dae # stub_end_effector_mini45.dae # tube_with_ati_collisions.dae # wedge_blender.dae # mesh.mesh_resource = "package://hrl_dynamic_mpc/meshes/tube_with_ati_collisions.dae" mesh.mesh_resource = "package://hrl_dynamic_mpc/meshes/Darci_Flipper.stl" mesh.type = Marker.MESH_RESOURCE #rot_default = tr.Ry(np.radians(-90))*tr.Rz(np.radians(90)) rot_default = tr.Ry(np.radians(0))*tr.Rz(np.radians(90)) if orientation == None: orientation = [0, 0, 0, 1] rot_buf = tr.quaternion_to_matrix(orientation) orientation = tr.matrix_to_quaternion(rot_buf*rot_default) mesh.pose.orientation = Quaternion(*orientation) mesh.pose.position.x = offset if control != None: control.markers.append( mesh ) elif marker_array != None: mesh.pose.position = ps.pose.position mesh.header.frame_id = ps.header.frame_id mesh.id = mesh_id marker_array.markers.append(mesh) if control != None: control.interaction_mode = InteractiveMarkerControl.BUTTON return control elif marker_array != None: return marker_array
def pose_cb(data, d): global t_pose_cb t_pose_cb = time.time() for obj in data.objects: p = obj.pose.position pos = np.matrix([p.x, p.y, p.z]).T q = obj.pose.orientation rot = tr.quaternion_to_matrix([q.x, q.y, q.z, q.w]) t = data.header.stamp.to_sec() d[obj.type]['pos_list'].append(pos) d[obj.type]['rot_list'].append(rot) d[obj.type]['time_list'].append(t) print '>>>>>>>>>>>>>>>>>>>>>>>>>>>>' print 'pos:', pos.T print 'rot:', rot
def transformTaxelArray(self, ta_msg, new_frame): # Get the transformation from the desired frame to current frame if ta_msg.header.frame_id == "": return ta_msg self.tf_lstnr.waitForTransform(new_frame, ta_msg.header.frame_id, rospy.Time(0), rospy.Duration(4.0)) t1, q1 = self.tf_lstnr.lookupTransform(new_frame, ta_msg.header.frame_id, rospy.Time(0)) t1 = np.matrix(t1).reshape(3,1) r1 = tr.quaternion_to_matrix(q1) # Create new message data structure new_ta_msg = copy.copy(ta_msg) new_ta_msg.header.frame_id = new_frame # Perform the transformation pts = np.column_stack((ta_msg.centers_x, ta_msg.centers_y, ta_msg.centers_z)) nrmls = np.column_stack((ta_msg.normals_x, ta_msg.normals_y, ta_msg.normals_z)) values = np.column_stack((ta_msg.values_x, ta_msg.values_y, ta_msg.values_z)) pts = r1 * np.matrix(pts).T + t1 nrmls = r1 * np.matrix(nrmls).T values = r1 * np.matrix(values).T # Reformat the transformed data to be repackaged as a TaxelArray message pts_array = np.asarray(pts) nrmls_array = np.asarray(nrmls) values_array = np.asarray(values) new_ta_msg.centers_x = pts_array[0, :].tolist() new_ta_msg.centers_y = pts_array[1, :].tolist() new_ta_msg.centers_z = pts_array[2, :].tolist() new_ta_msg.normals_x = nrmls_array[0, :].tolist() new_ta_msg.normals_y = nrmls_array[1, :].tolist() new_ta_msg.normals_z = nrmls_array[2, :].tolist() new_ta_msg.values_x = values_array[0, :].tolist() new_ta_msg.values_y = values_array[1, :].tolist() new_ta_msg.values_z = values_array[2, :].tolist() return new_ta_msg
def l_cart_state_cb(self, msg): ros_pt = msg.x_desi_filtered.pose.position self.l_cep_pos = np.matrix([ros_pt.x, ros_pt.y, ros_pt.z]).T ros_quat = msg.x_desi_filtered.pose.orientation quat = (ros_quat.x, ros_quat.y, ros_quat.z, ros_quat.w) self.l_cep_rot = tr.quaternion_to_matrix(quat)
def make_pr2_gripper_marker(ps, color, orientation = None, marker_array=None, control=None, mesh_id_start = 0, ns = "pr2_gripper", offset=-0.19): mesh_id = mesh_id_start # this is the palm piece mesh = Marker() mesh.ns = ns #mesh.mesh_use_embedded_materials = True mesh.scale = Vector3(1.,1.,1.) mesh.color = ColorRGBA(*color) mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/gripper_palm.dae" mesh.type = Marker.MESH_RESOURCE if orientation != None: mesh.pose.orientation = Quaternion(*orientation) else: mesh.pose.orientation = Quaternion(0,0,0,1) mesh.pose.position.x = offset if control != None: control.markers.append( mesh ) elif marker_array != None: mesh.pose.position = ps.pose.position mesh.header.frame_id = ps.header.frame_id mesh.id = mesh_id mesh_id = mesh_id+1 marker_array.markers.append(mesh) # amount to open the gripper for each finger angle_open = 0.4 if orientation == None: rot0 = np.matrix(np.eye(3)) else: rot0 = tr.quaternion_to_matrix(orientation) if marker_array != None: T0 = tr.composeHomogeneousTransform(rot0, [ps.point.x, ps.point.y, ps.point.z]) else: T0 = tr.composeHomogeneousTransform(rot0, [offset, 0.0, 0.]) #transforming the left finger and finger tip rot1 = tr.rot_angle_direction(angle_open, np.matrix([0, 0, 1])) T1 = tr.composeHomogeneousTransform(rot1, [0.07691, 0.01, 0.]) rot2 = tr.rot_angle_direction(-1*angle_open, np.matrix([0, 0, 1])) T2 = tr.composeHomogeneousTransform(rot2, [0.09137, 0.00495, 0.]) T_proximal = T0*T1 T_distal = T0*T1*T2 finger_pos = tr.tft.translation_from_matrix(T_proximal) finger_rot = tr.tft.quaternion_from_matrix(T_proximal) tip_pos = tr.tft.translation_from_matrix(T_distal) tip_rot = tr.tft.quaternion_from_matrix(T_distal) #making the marker for the left finger mesh = Marker() mesh.ns = ns #mesh.mesh_use_embedded_materials = True mesh.scale = Vector3(1.,1.,1.) mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae" mesh.color = ColorRGBA(*color) mesh.type = Marker.MESH_RESOURCE mesh.pose.orientation = Quaternion(*finger_rot) mesh.pose.position = Point(*finger_pos) if control != None: control.markers.append( mesh ) elif marker_array != None: mesh.header.frame_id = ps.header.frame_id mesh.id = mesh_id mesh_id = mesh_id+1 marker_array.markers.append(mesh) mesh = Marker() mesh.ns = ns #mesh.mesh_use_embedded_materials = True mesh.scale = Vector3(1.,1.,1.) mesh.color = ColorRGBA(*color) mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae" mesh.type = Marker.MESH_RESOURCE mesh.pose.orientation = Quaternion(*tip_rot) mesh.pose.position = Point(*tip_pos) if control != None: control.markers.append( mesh ) elif marker_array != None: mesh.header.frame_id = ps.header.frame_id mesh.id = mesh_id mesh_id = mesh_id+1 marker_array.markers.append(mesh) #transforming the right finger and finger tip rot1 = tr.rot_angle_direction(3.14, np.matrix([1, 0, 0]))*tr.rot_angle_direction(angle_open, np.matrix([0, 0, 1])) T1 = tr.composeHomogeneousTransform(rot1, [0.07691, -0.01, 0.]) rot2 = tr.rot_angle_direction(-1*angle_open, np.matrix([0, 0, 1])) T2 = tr.composeHomogeneousTransform(rot2, [0.09137, 0.00495, 0.]) T_proximal = T0*T1 T_distal = T0*T1*T2 finger_pos = tr.tft.translation_from_matrix(T_proximal) finger_rot = tr.tft.quaternion_from_matrix(T_proximal) tip_pos = tr.tft.translation_from_matrix(T_distal) tip_rot = tr.tft.quaternion_from_matrix(T_distal) #making the marker for the right finger mesh = Marker() mesh.ns = ns #mesh.mesh_use_embedded_materials = True mesh.scale = Vector3(1.,1.,1.) mesh.color = ColorRGBA(*color) mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae" mesh.type = Marker.MESH_RESOURCE mesh.pose.orientation = Quaternion(*finger_rot) mesh.pose.position = Point(*finger_pos) if control != None: control.markers.append( mesh ) elif marker_array != None: mesh.header.frame_id = ps.header.frame_id mesh.id = mesh_id mesh_id = mesh_id+1 marker_array.markers.append(mesh) mesh = Marker() mesh.ns = ns #mesh.mesh_use_embedded_materials = True mesh.scale = Vector3(1.,1.,1.) mesh.color = ColorRGBA(*color) mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae" mesh.type = Marker.MESH_RESOURCE mesh.pose.orientation = Quaternion(*tip_rot) mesh.pose.position = Point(*tip_pos) if control != None: control.markers.append( mesh ) elif marker_array != None: mesh.header.frame_id = ps.header.frame_id mesh.id = mesh_id mesh_id = mesh_id+1 marker_array.markers.append(mesh) if control != None: control.interaction_mode = InteractiveMarkerControl.BUTTON return control elif marker_array != None: return marker_array
def make_pr2_gripper_marker(ps, color, orientation=None, marker_array=None, control=None, mesh_id_start=0, ns="pr2_gripper", offset=-0.19): mesh_id = mesh_id_start # this is the palm piece mesh = Marker() mesh.ns = ns #mesh.mesh_use_embedded_materials = True mesh.scale = Vector3(1., 1., 1.) mesh.color = ColorRGBA(*color) mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/gripper_palm.dae" mesh.type = Marker.MESH_RESOURCE if orientation != None: mesh.pose.orientation = Quaternion(*orientation) else: mesh.pose.orientation = Quaternion(0, 0, 0, 1) mesh.pose.position.x = offset if control != None: control.markers.append(mesh) elif marker_array != None: mesh.pose.position = ps.pose.position mesh.header.frame_id = ps.header.frame_id mesh.id = mesh_id mesh_id = mesh_id + 1 marker_array.markers.append(mesh) # amount to open the gripper for each finger angle_open = 0.4 if orientation == None: rot0 = np.matrix(np.eye(3)) else: rot0 = tr.quaternion_to_matrix(orientation) if marker_array != None: T0 = tr.composeHomogeneousTransform( rot0, [ps.point.x, ps.point.y, ps.point.z]) else: T0 = tr.composeHomogeneousTransform(rot0, [offset, 0.0, 0.]) #transforming the left finger and finger tip rot1 = tr.rot_angle_direction(angle_open, np.matrix([0, 0, 1])) T1 = tr.composeHomogeneousTransform(rot1, [0.07691, 0.01, 0.]) rot2 = tr.rot_angle_direction(-1 * angle_open, np.matrix([0, 0, 1])) T2 = tr.composeHomogeneousTransform(rot2, [0.09137, 0.00495, 0.]) T_proximal = T0 * T1 T_distal = T0 * T1 * T2 finger_pos = tr.tft.translation_from_matrix(T_proximal) finger_rot = tr.tft.quaternion_from_matrix(T_proximal) tip_pos = tr.tft.translation_from_matrix(T_distal) tip_rot = tr.tft.quaternion_from_matrix(T_distal) #making the marker for the left finger mesh = Marker() mesh.ns = ns #mesh.mesh_use_embedded_materials = True mesh.scale = Vector3(1., 1., 1.) mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae" mesh.color = ColorRGBA(*color) mesh.type = Marker.MESH_RESOURCE mesh.pose.orientation = Quaternion(*finger_rot) mesh.pose.position = Point(*finger_pos) if control != None: control.markers.append(mesh) elif marker_array != None: mesh.header.frame_id = ps.header.frame_id mesh.id = mesh_id mesh_id = mesh_id + 1 marker_array.markers.append(mesh) mesh = Marker() mesh.ns = ns #mesh.mesh_use_embedded_materials = True mesh.scale = Vector3(1., 1., 1.) mesh.color = ColorRGBA(*color) mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae" mesh.type = Marker.MESH_RESOURCE mesh.pose.orientation = Quaternion(*tip_rot) mesh.pose.position = Point(*tip_pos) if control != None: control.markers.append(mesh) elif marker_array != None: mesh.header.frame_id = ps.header.frame_id mesh.id = mesh_id mesh_id = mesh_id + 1 marker_array.markers.append(mesh) #transforming the right finger and finger tip rot1 = tr.rot_angle_direction(3.14, np.matrix( [1, 0, 0])) * tr.rot_angle_direction(angle_open, np.matrix([0, 0, 1])) T1 = tr.composeHomogeneousTransform(rot1, [0.07691, -0.01, 0.]) rot2 = tr.rot_angle_direction(-1 * angle_open, np.matrix([0, 0, 1])) T2 = tr.composeHomogeneousTransform(rot2, [0.09137, 0.00495, 0.]) T_proximal = T0 * T1 T_distal = T0 * T1 * T2 finger_pos = tr.tft.translation_from_matrix(T_proximal) finger_rot = tr.tft.quaternion_from_matrix(T_proximal) tip_pos = tr.tft.translation_from_matrix(T_distal) tip_rot = tr.tft.quaternion_from_matrix(T_distal) #making the marker for the right finger mesh = Marker() mesh.ns = ns #mesh.mesh_use_embedded_materials = True mesh.scale = Vector3(1., 1., 1.) mesh.color = ColorRGBA(*color) mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae" mesh.type = Marker.MESH_RESOURCE mesh.pose.orientation = Quaternion(*finger_rot) mesh.pose.position = Point(*finger_pos) if control != None: control.markers.append(mesh) elif marker_array != None: mesh.header.frame_id = ps.header.frame_id mesh.id = mesh_id mesh_id = mesh_id + 1 marker_array.markers.append(mesh) mesh = Marker() mesh.ns = ns #mesh.mesh_use_embedded_materials = True mesh.scale = Vector3(1., 1., 1.) mesh.color = ColorRGBA(*color) mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae" mesh.type = Marker.MESH_RESOURCE mesh.pose.orientation = Quaternion(*tip_rot) mesh.pose.position = Point(*tip_pos) if control != None: control.markers.append(mesh) elif marker_array != None: mesh.header.frame_id = ps.header.frame_id mesh.id = mesh_id mesh_id = mesh_id + 1 marker_array.markers.append(mesh) if control != None: control.interaction_mode = InteractiveMarkerControl.BUTTON return control elif marker_array != None: return marker_array