00001
00002 import sys
00003 import numpy as np, math
00004
00005 import add_cylinder as ac
00006 import online_collision_detection as ocd
00007 import force_visualize_test as fvt
00008
00009 import roslib; roslib.load_manifest('arm_navigation_tutorials')
00010 import rospy
00011 from mapping_msgs.msg import CollisionObject
00012 from visualization_msgs.msg import Marker
00013
00014 import hrl_lib.viz as hv
00015
00016 roslib.load_manifest('hrl_pr2_lib')
00017 import hrl_pr2_lib.pr2_arms as pa
00018
00019
00020 def contact_info_list_to_dict(cont_info_list):
00021 ci = cont_info_list[0]
00022 frm = ci.header.frame_id
00023
00024 b1 = ci.contact_body_1
00025 b2 = ci.contact_body_2
00026 contact_dict = {}
00027 pts_list = []
00028 for ci in cont_info_list:
00029 if frm != ci.header.frame_id:
00030 rospy.logerr('initial frame_id: %s and current frame_id: %s'%(frm, ci.header.frame_id))
00031
00032 b1 = ci.contact_body_1
00033 b2 = ci.contact_body_2
00034 two_bodies = b1 + '+' + b2
00035 if two_bodies not in contact_dict:
00036 contact_dict[two_bodies] = []
00037 contact_dict[two_bodies].append((ci.position.x, ci.position.y, ci.position.z))
00038
00039 return contact_dict
00040
00041 def visualize_contact_dict(cd, marker_pub, fts):
00042 color_list = [(1.,0.,0.), (0.,1.,0.), (0.,0.,1.), (1.,1.,0.),
00043 (1.,0.,1.), (0.,1.,1.), (0.5,1.,0.), (0.5,0.,1.),
00044 (0.,0.5,1.) ]
00045 pts_list = []
00046 cs_list = []
00047 marker_list = []
00048 for i, k in enumerate(cd.keys()):
00049 pts = np.matrix(cd[k]).T
00050 c = color_list[i]
00051 cs = np.ones((4, pts.shape[1]))
00052 cs[0,:] = c[0]
00053 cs[1,:] = c[1]
00054 cs[2,:] = c[2]
00055 pts_list.append(pts)
00056 cs_list.append(cs)
00057 print '# of contact points:', pts.shape[1]
00058 mn = np.mean(pts, 1)
00059
00060 f = fts.get_forces()
00061 m1, m2 = fvt.get_arrow_text_markers(mn, f, 'base_footprint',
00062 m_id = 2*i+1, duration=0.5)
00063 marker_pub.publish(m1)
00064 marker_pub.publish(m2)
00065
00066 m = hv.list_marker(np.column_stack(pts_list),
00067 np.column_stack(cs_list), (0.01, 0.01, 0.01),
00068 'points', 'base_footprint', duration=1.0,
00069 m_id=0)
00070 t_now = rospy.Time.now()
00071 m.header.stamp = t_now
00072 marker_pub.publish(m)
00073
00074 for m in marker_list:
00075 m.header.stamp = rospy.Time.now()
00076 marker_pub.publish(m)
00077
00078
00079 if __name__ == '__main__':
00080 rospy.init_node('pr2_skin_simulate')
00081 pub = rospy.Publisher('collision_object', CollisionObject)
00082 marker_pub = rospy.Publisher('/skin/viz_marker', Marker)
00083
00084 fts = fvt.object_ft_sensors()
00085 fts.bias_fts()
00086 pr2_arms = pa.PR2Arms()
00087 r_arm, l_arm = 0, 1
00088 arm = r_arm
00089
00090 raw_input('Touch the object and then hit ENTER.')
00091
00092 ee_pos, ee_rot = pr2_arms.end_effector_pos(arm)
00093 print 'ee_pos:', ee_pos.flatten()
00094 print 'ee_pos.shape:', ee_pos.shape
00095
00096
00097 trans, quat = pr2_arms.tf_lstnr.lookupTransform('/base_footprint',
00098 '/torso_lift_link', rospy.Time(0))
00099 height = ee_pos[2] + trans[2]
00100 ee_pos[2] = -trans[2]
00101 ac.add_cylinder('pole', ee_pos, 0.02, height, '/torso_lift_link', pub)
00102
00103 rospy.loginfo('Now starting the loop where I get contact locations.')
00104 col_det = ocd.online_collision_detector()
00105 while not rospy.is_shutdown():
00106 rospy.sleep(0.1)
00107 res = col_det.check_validity(pr2_arms, arm)
00108 if res.error_code.val == res.error_code.SUCCESS:
00109 rospy.loginfo('No contact')
00110 else:
00111 contact_dict = contact_info_list_to_dict(res.contacts)
00112 print 'contact_dict.keys:', contact_dict.keys()
00113 visualize_contact_dict(contact_dict, marker_pub, fts)
00114
00115
00116