hrl_software_simulation_darpa_m3: draw

Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import numpy as np
00004 import roslib
00005 roslib.load_manifest('hrl_software_simulation_darpa_m3')
00006 import rospy
00007 
00008 import hrl_common_code_darpa_m3.visualization.draw_scene as ds
00009 
00010 from hrl_haptic_manipulation_in_clutter_msgs.msg import BodyDraw
00011 from hrl_haptic_manipulation_in_clutter_msgs.msg import JtsDraw
00012 from hrl_msgs.msg import FloatArrayBare
00013 from hrl_lib.transforms import *
00014 import hrl_lib.viz as hv
00015 import threading
00016 
00017 from visualization_msgs.msg import Marker
00018 
00019 
00020 class DrawAll:
00021     def __init__(self):
00022         self.lock = threading.RLock()
00023         rospy.init_node('draw_all_bodies')
00024 
00025         # this needs to be the first param that is read for
00026         # synchronization with obstacles.py
00027         while not rospy.is_shutdown():
00028             try:
00029                 self.moveable_dimen = rospy.get_param('m3/software_testbed/movable_dimen')
00030                 break
00031             except KeyError:
00032                 rospy.sleep(0.1)
00033                 continue
00034             
00035         self.fixed_dimen = rospy.get_param('m3/software_testbed/fixed_dimen')
00036         self.compliant_dimen = rospy.get_param('m3/software_testbed/compliant_dimen')
00037         self.num_fixed = rospy.get_param('m3/software_testbed/num_fixed')
00038         self.num_moveable = rospy.get_param('m3/software_testbed/num_movable')
00039         self.num_compliant = rospy.get_param('m3/software_testbed/num_compliant')
00040         self.num_tot = rospy.get_param('m3/software_testbed/num_total')
00041 
00042         self.fixed_ctype = rospy.get_param('m3/software_testbed/fixed_ctype')
00043         self.movable_ctype = rospy.get_param('m3/software_testbed/movable_ctype')
00044         
00045         self.goal = np.matrix(rospy.get_param('m3/software_testbed/goal')).T
00046 
00047         try:
00048             self.movable_stiffness = rospy.get_param('m3/software_testbed/compliant_stiffness')
00049         except KeyError:
00050             self.movable_stiffness = [-1]*int(self.num_compliant)
00051 
00052         # all params from the robot_config uploading node (e.g.
00053         # three_link_planar_param_upload.py) should accessed after
00054         # this loop.
00055         while not rospy.is_shutdown():
00056             try:
00057                 self.num_links = rospy.get_param('/m3/software_testbed/linkage/num_links')
00058                 break
00059             except KeyError:
00060                 rospy.sleep(0.1)
00061                 continue
00062 
00063         self.links_dim = rospy.get_param('/m3/software_testbed/linkage/dimensions')
00064         self.links_shape = rospy.get_param('/m3/software_testbed/linkage/shapes')
00065 
00066         self.joints_pos = []
00067         self.active_jts = None
00068         self.constr_jts = None
00069         self.links_pos = []
00070         self.links_rot = []
00071         self.obst_pos = []
00072         self.obst_rot = []
00073         self.draw_obstacles = ds.SceneDraw("sim/viz/obstacles", "/torso_lift_link")
00074         self.draw_links = ds.SceneDraw("sim/viz/linkage", "/torso_lift_link")
00075         self.color_active = [0, 1, 0, 1]
00076         self.color_limit = [1, 0, 0, 1]
00077         self.color_passive = [0, 0, 1, 1]
00078         self.color_links =rospy.get_param('m3/software_testbed/linkage/colors')
00079         rospy.Subscriber("/sim_arm/bodies_visualization", BodyDraw, self.bodies_callback)
00080 
00081         self.goal_marker_pub = rospy.Publisher('/epc_skin/viz/goal', Marker)
00082 
00083         
00084     def bodies_callback(self, msg):
00085         with self.lock:
00086             self.links_pos = msg.link_loc
00087             self.links_rot = msg.link_rot
00088             self.obst_pos = msg.obst_loc
00089             self.obst_rot = msg.obst_rot
00090 
00091     def draw_bodies(self):
00092         obst_counter = 0
00093         link_counter = 0
00094         color_active = self.color_active
00095         color_limit = self.color_limit
00096         color_passive = self.color_passive
00097         color_links = self.color_links
00098         jt_size = 0.05
00099 
00100         self.lock.acquire()
00101         for i in xrange(len(self.links_pos)):
00102             link_rot_mat = self.draw_links.get_rot_mat(self.links_rot[i].data).T
00103             if self.links_shape[i] == "cube":
00104                 shape = self.draw_links.Marker.CUBE
00105                 dims = self.links_dim[i]
00106             elif self.links_shape[i] == "capsule":
00107                 shape = self.draw_links.Marker.CYLINDER
00108                 dims = self.links_dim[i]
00109                 #dims = [self.links_dim[i][0], self.links_dim[i][1], self.links_dim[i][2]]
00110                 dim_jt = [self.links_dim[i][0], self.links_dim[i][0], self.links_dim[i][0]]
00111                 jt_pos_loc = link_rot_mat*np.array([0, 0, self.links_dim[i][2]/2.0]).reshape(3,1)
00112                 jt_pos_plus = np.array(self.links_pos[i].data).reshape(3,1) + jt_pos_loc
00113                 jt_pos_neg = np.array(self.links_pos[i].data).reshape(3,1) - jt_pos_loc                
00114                 self.draw_links.pub_body(jt_pos_plus, matrix_to_quaternion(link_rot_mat), dim_jt, color_links[i], link_counter, self.draw_links.Marker.SPHERE)
00115                 self.draw_links.pub_body(jt_pos_neg, matrix_to_quaternion(link_rot_mat), dim_jt, color_links[i], link_counter+1, self.draw_links.Marker.SPHERE)
00116                 link_counter=link_counter+2
00117 
00118             self.draw_links.pub_body(self.links_pos[i].data, matrix_to_quaternion(self.draw_links.get_rot_mat(self.links_rot[i].data).T),
00119                                dims, color_links[i], link_counter, shape)
00120             link_counter = link_counter + 1
00121 
00122         if self.obst_pos != []:
00123             for i in xrange(self.num_moveable):
00124                 color = [0.6,0.6,0,0.7]
00125                 self.draw_obstacles.pub_body(self.obst_pos[i].data, 
00126                                    matrix_to_quaternion(self.draw_obstacles.get_rot_mat(self.obst_rot[i].data).T),
00127                                    [self.moveable_dimen[i][0]*2, self.moveable_dimen[i][1]*2, self.moveable_dimen[i][2]], 
00128                                    color, 
00129                                    obst_counter, 
00130                                    self.draw_obstacles.Marker.CYLINDER)
00131                 obst_counter = obst_counter + 1
00132 
00133             for i in xrange(self.num_compliant):
00134                 color = [0, 100/255.0, 0, 0.9]
00135                 self.draw_obstacles.pub_body(self.obst_pos[self.num_moveable+i].data, 
00136                                    matrix_to_quaternion(self.draw_obstacles.get_rot_mat(self.obst_rot[self.num_moveable+i].data).T),
00137                                    [self.compliant_dimen[i][0]*2, self.compliant_dimen[i][1]*2, self.compliant_dimen[i][2]], 
00138                                    color, 
00139                                    obst_counter, 
00140                                    self.draw_obstacles.Marker.CYLINDER)
00141                 obst_counter = obst_counter + 1
00142 
00143 
00144             for i in xrange(self.num_fixed):
00145                 color2 = [0.3, 0.,0.,0.7]
00146                     
00147                 if self.fixed_ctype[i] == 'wall':
00148                     
00149                     obst_rot_mat = self.draw_links.get_rot_mat(self.obst_rot[i].data).T
00150                     ## dim_jt       = [self.fixed_dimen[i][1]/2.0, self.fixed_dimen[i][1]/2.0, self.fixed_dimen[i][2]]
00151 
00152                     ## #jt_pos_loc = link_rot_mat*np.array([0, 0, self.links_dim[i][2]/2.0]).reshape(3,1)
00153                     ## #jt_pos_plus = np.array(self.links_pos[i].data).reshape(3,1) + jt_pos_loc
00154                     ## #jt_pos_neg = np.array(self.links_pos[i].data).reshape(3,1) - jt_pos_loc                                    
00155 
00156                     ## jt_pos_loc  = obst_rot_mat*np.array([0, 0, self.fixed_dimen[i][2]/2.0]).reshape(3,1)                    
00157                     ## jt_pos_plus = self.obst_pos[self.num_moveable+self.num_compliant+i].data + jt_pos_loc
00158                     ## jt_pos_neg  = self.obst_pos[self.num_moveable+self.num_compliant+i].data - jt_pos_loc                    
00159 
00160                     ## self.draw_links.pub_body(jt_pos_plus, matrix_to_quaternion(obst_rot_mat), dim_jt, color_links[i], link_counter, self.draw_links.Marker.SPHERE)
00161                     ## self.draw_links.pub_body(jt_pos_neg, matrix_to_quaternion(obst_rot_mat), dim_jt, color_links[i], link_counter+1, self.draw_links.Marker.SPHERE)
00162                     ## link_counter=link_counter+2
00163 
00164                     #print self.num_moveable+self.num_compliant+i
00165                     #print self.obst_rot[self.num_moveable+self.num_compliant+i].data
00166                     
00167                     self.draw_obstacles.pub_body(self.obst_pos[self.num_moveable+self.num_compliant+i].data, 
00168                                                  matrix_to_quaternion(self.draw_obstacles.get_rot_mat(self.obst_rot[self.num_moveable+self.num_compliant+i].data).T),
00169                                                  [self.fixed_dimen[i][0], self.fixed_dimen[i][1], self.fixed_dimen[i][2]], 
00170                                                  color2, 
00171                                                  obst_counter, 
00172                                                  self.draw_obstacles.Marker.CUBE)
00173                 else:                    
00174                     self.draw_obstacles.pub_body(self.obst_pos[self.num_moveable+self.num_compliant+i].data, 
00175                                                  matrix_to_quaternion(self.draw_obstacles.get_rot_mat(self.obst_rot[self.num_moveable+self.num_compliant+i].data).T),
00176                                                  [self.fixed_dimen[i][0]*2, self.fixed_dimen[i][1]*2, self.fixed_dimen[i][2]], 
00177                                                  color2, 
00178                                                  obst_counter, 
00179                                                  self.draw_obstacles.Marker.CYLINDER)
00180                 obst_counter = obst_counter + 1
00181         #self.num_fixed = rospy.get_param('m3/software_testbed/num_fixed')
00182 
00183         self.lock.release()
00184 
00185     def publish_goal_marker(self, goal_pos):
00186         o = np.matrix([0.,0.,0.,1.]).T
00187         g_m = hv.single_marker(goal_pos, o, 'sphere',
00188                                '/world', color=(0., 1., 1., 1.),
00189                                scale = (0.02, 0.02, 0.02),
00190                                m_id = 23, duration=0)
00191         g_m.header.stamp = rospy.Time.now()
00192         self.goal_marker_pub.publish(g_m)    
00193         #self.draw.pub.publish(g_m)
00194 
00195 
00196 
00197 if __name__ == '__main__':
00198     draw_stuff = DrawAll()
00199     rospy.loginfo('Started visualizing bodies!')
00200     while not rospy.is_shutdown():
00201         draw_stuff.draw_bodies()
00202         draw_stuff.publish_goal_marker(draw_stuff.goal)
00203         rospy.sleep(0.1)
00204 
00205                         
00206 
00207