hrl_haptic_mpc: interactive_marker

Go to the documentation of this file.
00001 
00002 #
00003 # Code copied from basic_controls.py and then modified.
00004 # Author: Marc Killpack
00005 
00006 
00007 """
00008 Copyright (c) 2011, Willow Garage, Inc.
00009 All rights reserved.
00010 
00011 Redistribution and use in source and binary forms, with or without
00012 modification, are permitted provided that the following conditions are met:
00013 
00014     * Redistributions of source code must retain the above copyright
00015       notice, this list of conditions and the following disclaimer.
00016     * Redistributions in binary form must reproduce the above copyright
00017       notice, this list of conditions and the following disclaimer in the
00018       documentation and/or other materials provided with the distribution.
00019     * Neither the name of the Willow Garage, Inc. nor the names of its
00020       contributors may be used to endorse or promote products derived from
00021       this software without specific prior written permission.
00022 
00023 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00024 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00025 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00026 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00027 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00028 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00029 SUBSTITUTE GOODS OR SERVICES LOSS OF USE, DATA, OR PROFITS OR BUSINESS
00030 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00031 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00032 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 POSSIBILITY OF SUCH DAMAGE.
00034 """
00035 
00036 import math, numpy as np
00037 
00038 import roslib; roslib.load_manifest('interactive_markers')
00039 import rospy
00040 roslib.load_manifest('hrl_lib')
00041 from hrl_lib import transforms as tr
00042 
00043 from visualization_msgs.msg import Marker, InteractiveMarker, InteractiveMarkerControl
00044 
00045 
00046 # scale - float
00047 # color - (r,g,b,a)
00048 # mtype - 'cube', 'sphere'
00049 def make_marker(scale, color, mtype):
00050     ss = 0.3
00051     marker = Marker()
00052     if mtype == 'cube':
00053         marker.type = Marker.CUBE
00054         ss = ss * 1/(math.pow(3, 1./3))
00055     elif mtype == 'sphere':
00056         marker.type = Marker.SPHERE
00057     elif mtype == 'cylinder':
00058         marker.type = Marker.CYLINDER        
00059     else:
00060         raise RuntimeError('Undefined marker type')
00061 
00062     marker.scale.x = ss * scale
00063     marker.scale.y = ss * scale
00064     marker.scale.z = ss * scale
00065 
00066     marker.color.r = color[0]
00067     marker.color.g = color[1]
00068     marker.color.b = color[2]
00069     marker.color.a = color[3]
00070     return marker
00071 
00072 def make_3dof_marker_position(ps, scale, color, mtype):
00073     return make_marker_flexible(True, ps, scale, color, mtype,
00074                                 ignore_rotation = True)
00075 
00076 def make_marker_position_xy(ps, scale, color, mtype):
00077     return make_marker_flexible(True, ps, scale, color, mtype,
00078                                 ignore_rotation=True, ignore_z=True)
00079 
00080 def make_cody_ee_marker(ps, color, orientation = None,
00081                             marker_array=None, control=None, 
00082                             mesh_id_start = 0, ns = "cody_ee",
00083                             offset=0.08):
00084     mesh_id = mesh_id_start
00085     # this is the palm piece
00086     mesh = Marker()
00087     mesh.ns = ns
00088     #mesh.mesh_use_embedded_materials = True
00089     mesh.scale.x = 1.0
00090     mesh.scale.y = 1.0
00091     mesh.scale.z = 1.0
00092     mesh.color.r = color[0]
00093     mesh.color.g = color[1]
00094     mesh.color.b = color[2]
00095     mesh.color.a = color[3]
00096 
00097     # stub_end_effector.dae
00098     # stub_end_effector_mini45.dae
00099     # tube_with_ati_collisions.dae
00100     # wedge_blender.dae
00101     mesh.mesh_resource = "package://cody/urdf/tube_with_ati_collisions.dae"
00102     mesh.type = Marker.MESH_RESOURCE
00103 
00104     rot_default = tr.Ry(np.radians(-90))*tr.Rz(np.radians(90))
00105 
00106     if orientation == None:
00107         orientation = [0, 0, 0, 1]
00108 
00109     rot_buf = tr.quaternion_to_matrix(orientation)
00110     orientation = tr.matrix_to_quaternion(rot_buf*rot_default)
00111     mesh.pose.orientation.x = orientation[0]
00112     mesh.pose.orientation.y = orientation[1]
00113     mesh.pose.orientation.z = orientation[2]
00114     mesh.pose.orientation.w = orientation[3]
00115 
00116     mesh.pose.position.z = offset
00117     if control != None:
00118         control.markers.append( mesh )
00119     elif marker_array != None:
00120         mesh.pose.position.x = ps.point.x
00121         mesh.pose.position.y = ps.point.y
00122         mesh.pose.position.z = ps.point.z
00123         mesh.header.frame_id = ps.header.frame_id
00124         mesh.id = mesh_id
00125         marker_array.markers.append(mesh)
00126 
00127     if control != None:
00128         control.interaction_mode = InteractiveMarkerControl.BUTTON
00129         return control
00130     elif marker_array != None:
00131         return marker_array
00132 
00133 
00134 def make_pr2_gripper_marker(ps, color, orientation = None,
00135                             marker_array=None, control=None, 
00136                             mesh_id_start = 0, ns = "pr2_gripper",
00137                             offset=-0.17):
00138     mesh_id = mesh_id_start
00139     # this is the palm piece
00140     mesh = Marker()
00141     mesh.ns = ns
00142     #mesh.mesh_use_embedded_materials = True
00143     mesh.scale.x = 1.0
00144     mesh.scale.y = 1.0
00145     mesh.scale.z = 1.0
00146     mesh.color.r = color[0]
00147     mesh.color.g = color[1]
00148     mesh.color.b = color[2]
00149     mesh.color.a = color[3]
00150     mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/gripper_palm.dae"
00151     mesh.type = Marker.MESH_RESOURCE
00152     if orientation != None:
00153         mesh.pose.orientation.x = orientation[0]
00154         mesh.pose.orientation.y = orientation[1]
00155         mesh.pose.orientation.z = orientation[2]
00156         mesh.pose.orientation.w = orientation[3]
00157     else:
00158         mesh.pose.orientation.w = 1
00159     mesh.pose.position.x = offset
00160     if control != None:
00161         control.markers.append( mesh )
00162     elif marker_array != None:
00163         mesh.pose.position.x = ps.point.x
00164         mesh.pose.position.y = ps.point.y
00165         mesh.pose.position.z = ps.point.z
00166         mesh.header.frame_id = ps.header.frame_id
00167         mesh.id = mesh_id
00168         mesh_id = mesh_id+1
00169         marker_array.markers.append(mesh)
00170 
00171     # amount to open the gripper for each finger
00172     angle_open = 0.4
00173     if orientation == None:
00174         rot0 = np.matrix(np.eye(3))
00175     else:
00176         rot0 = tr.quaternion_to_matrix(orientation)
00177 
00178     if marker_array != None:
00179         T0 = tr.composeHomogeneousTransform(rot0, [ps.point.x, ps.point.y, ps.point.z])
00180     else:
00181         T0 = tr.composeHomogeneousTransform(rot0, [offset, 0.0, 0.])
00182 
00183     #transforming the left finger and finger tip
00184     rot1 = tr.rot_angle_direction(angle_open, np.matrix([0, 0, 1]))
00185     T1 = tr.composeHomogeneousTransform(rot1, [0.07691, 0.01, 0.])
00186     rot2 = tr.rot_angle_direction(-1*angle_open, np.matrix([0, 0, 1]))
00187     T2 = tr.composeHomogeneousTransform(rot2, [0.09137, 0.00495, 0.])
00188     
00189     T_proximal = T0*T1
00190     T_distal = T0*T1*T2
00191 
00192     finger_pos = tr.tft.translation_from_matrix(T_proximal)
00193     finger_rot = tr.tft.quaternion_from_matrix(T_proximal)
00194 
00195     tip_pos = tr.tft.translation_from_matrix(T_distal)
00196     tip_rot = tr.tft.quaternion_from_matrix(T_distal)
00197 
00198     #making the marker for the left finger
00199     mesh = Marker()
00200     mesh.ns = ns
00201     #mesh.mesh_use_embedded_materials = True
00202     mesh.scale.x = 1.0
00203     mesh.scale.y = 1.0
00204     mesh.scale.z = 1.0
00205     mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"
00206     mesh.color.r = color[0]
00207     mesh.color.g = color[1]
00208     mesh.color.b = color[2]
00209     mesh.color.a = color[3]
00210     mesh.type = Marker.MESH_RESOURCE
00211     mesh.pose.orientation.x = finger_rot[0] 
00212     mesh.pose.orientation.y = finger_rot[1] 
00213     mesh.pose.orientation.z = finger_rot[2] 
00214     mesh.pose.orientation.w = finger_rot[3]
00215     mesh.pose.position.x = finger_pos[0] 
00216     mesh.pose.position.y = finger_pos[1] 
00217     mesh.pose.position.z = finger_pos[2] 
00218     if control != None:
00219         control.markers.append( mesh )
00220     elif marker_array != None:
00221         mesh.header.frame_id = ps.header.frame_id
00222         mesh.id = mesh_id
00223         mesh_id = mesh_id+1
00224         marker_array.markers.append(mesh)
00225 
00226     mesh = Marker()
00227     mesh.ns = ns
00228     #mesh.mesh_use_embedded_materials = True
00229     mesh.scale.x = 1.0
00230     mesh.scale.y = 1.0
00231     mesh.scale.z = 1.0
00232     mesh.color.r = color[0]
00233     mesh.color.g = color[1]
00234     mesh.color.b = color[2]
00235     mesh.color.a = color[3]
00236     mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"
00237     mesh.type = Marker.MESH_RESOURCE
00238     mesh.pose.orientation.x = tip_rot[0] 
00239     mesh.pose.orientation.y = tip_rot[1] 
00240     mesh.pose.orientation.z = tip_rot[2] 
00241     mesh.pose.orientation.w = tip_rot[3]
00242     mesh.pose.position.x = tip_pos[0] 
00243     mesh.pose.position.y = tip_pos[1] 
00244     mesh.pose.position.z = tip_pos[2] 
00245     if control != None:
00246         control.markers.append( mesh )
00247     elif marker_array != None:
00248         mesh.header.frame_id = ps.header.frame_id
00249         mesh.id = mesh_id
00250         mesh_id = mesh_id+1
00251         marker_array.markers.append(mesh)
00252 
00253     #transforming the right finger and finger tip
00254     rot1 = tr.rot_angle_direction(3.14, np.matrix([1, 0, 0]))*tr.rot_angle_direction(angle_open, np.matrix([0, 0, 1]))
00255     T1 = tr.composeHomogeneousTransform(rot1, [0.07691, -0.01, 0.])
00256     rot2 = tr.rot_angle_direction(-1*angle_open, np.matrix([0, 0, 1]))
00257     T2 = tr.composeHomogeneousTransform(rot2, [0.09137, 0.00495, 0.])
00258     
00259     T_proximal = T0*T1
00260     T_distal = T0*T1*T2
00261 
00262     finger_pos = tr.tft.translation_from_matrix(T_proximal)
00263     finger_rot = tr.tft.quaternion_from_matrix(T_proximal)
00264 
00265     tip_pos = tr.tft.translation_from_matrix(T_distal)
00266     tip_rot = tr.tft.quaternion_from_matrix(T_distal)
00267 
00268     #making the marker for the right finger
00269     mesh = Marker()
00270     mesh.ns = ns
00271     #mesh.mesh_use_embedded_materials = True
00272     mesh.scale.x = 1.0
00273     mesh.scale.y = 1.0
00274     mesh.scale.z = 1.0
00275     mesh.color.r = color[0]
00276     mesh.color.g = color[1]
00277     mesh.color.b = color[2]
00278     mesh.color.a = color[3]
00279     mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"
00280     mesh.type = Marker.MESH_RESOURCE
00281     mesh.pose.orientation.x = finger_rot[0] 
00282     mesh.pose.orientation.y = finger_rot[1] 
00283     mesh.pose.orientation.z = finger_rot[2] 
00284     mesh.pose.orientation.w = finger_rot[3]
00285     mesh.pose.position.x = finger_pos[0] 
00286     mesh.pose.position.y = finger_pos[1] 
00287     mesh.pose.position.z = finger_pos[2] 
00288     if control != None:
00289         control.markers.append( mesh )
00290     elif marker_array != None:
00291         mesh.header.frame_id = ps.header.frame_id
00292         mesh.id = mesh_id
00293         mesh_id = mesh_id+1
00294         marker_array.markers.append(mesh)
00295 
00296     mesh = Marker()
00297     mesh.ns = ns
00298     #mesh.mesh_use_embedded_materials = True
00299     mesh.scale.x = 1.0
00300     mesh.scale.y = 1.0
00301     mesh.scale.z = 1.0
00302     mesh.color.r = color[0]
00303     mesh.color.g = color[1]
00304     mesh.color.b = color[2]
00305     mesh.color.a = color[3]
00306     mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"
00307     mesh.type = Marker.MESH_RESOURCE
00308     mesh.pose.orientation.x = tip_rot[0] 
00309     mesh.pose.orientation.y = tip_rot[1] 
00310     mesh.pose.orientation.z = tip_rot[2] 
00311     mesh.pose.orientation.w = tip_rot[3]
00312     mesh.pose.position.x = tip_pos[0] 
00313     mesh.pose.position.y = tip_pos[1] 
00314     mesh.pose.position.z = tip_pos[2] 
00315     if control != None:
00316         control.markers.append( mesh )
00317     elif marker_array != None:
00318         mesh.header.frame_id = ps.header.frame_id
00319         mesh.id = mesh_id
00320         mesh_id = mesh_id+1
00321         marker_array.markers.append(mesh)
00322 
00323     if control != None:
00324         control.interaction_mode = InteractiveMarkerControl.BUTTON
00325         return control
00326     elif marker_array != None:
00327         return marker_array
00328 
00329 def make_6dof_gripper(fixed, ps, scale, color, robot_type = "pr2",
00330                       ignore_rotation = False, ignore_x=False,
00331                       ignore_y=False, ignore_z=False):
00332     int_marker = InteractiveMarker()
00333     int_marker.header.frame_id = ps.header.frame_id
00334     int_marker.pose.position.x = ps.point.x
00335     int_marker.pose.position.y = ps.point.y
00336     int_marker.pose.position.z = ps.point.z
00337     int_marker.scale = scale
00338 
00339     int_marker.name = 'gripper_6dof'
00340 
00341     control =  InteractiveMarkerControl()
00342     control.always_visible = True
00343     if robot_type == "pr2":
00344         control = make_pr2_gripper_marker(ps, [0.3, 0.3, 0.3, 0.7], control=control) 
00345         int_marker.description = 'pr2_gripper_control'
00346     elif robot_type == "cody":
00347         control = make_cody_ee_marker(ps, [1, 1, 1, 0.4], control=control) 
00348         int_marker.description = 'cody_ee_control'
00349     int_marker.controls.append( control )
00350 
00351     if not ignore_x:
00352         control = InteractiveMarkerControl()
00353         control.orientation.x = 1
00354         control.orientation.y = 0
00355         control.orientation.z = 0
00356         control.orientation.w = 1
00357         control.name = 'move_x'
00358         control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00359         if fixed:
00360             control.orientation_mode = InteractiveMarkerControl.FIXED
00361         int_marker.controls.append(control)
00362 
00363     if not ignore_y:
00364         control = InteractiveMarkerControl()
00365         control.orientation.x = 0
00366         control.orientation.y = 0
00367         control.orientation.z = 1
00368         control.orientation.w = 1
00369         control.name = 'move_y'
00370         control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00371         if fixed:
00372             control.orientation_mode = InteractiveMarkerControl.FIXED
00373         int_marker.controls.append(control)
00374 
00375     if not ignore_z:
00376         control = InteractiveMarkerControl()
00377         control.orientation.x = 0
00378         control.orientation.y = 1
00379         control.orientation.z = 0
00380         control.orientation.w = 1
00381         control.name = 'move_z'
00382         control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00383         if fixed:
00384             control.orientation_mode = InteractiveMarkerControl.FIXED
00385         int_marker.controls.append(control)
00386 
00387     if not ignore_rotation:
00388         control = InteractiveMarkerControl()
00389         control.orientation.x = 1
00390         control.orientation.y = 0
00391         control.orientation.z = 0
00392         control.orientation.w = 1
00393         control.name = 'rotate_x'
00394         control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00395         if fixed:
00396             control.orientation_mode = InteractiveMarkerControl.FIXED
00397         int_marker.controls.append(control)
00398 
00399         control = InteractiveMarkerControl()
00400         control.orientation.x = 0
00401         control.orientation.y = 0
00402         control.orientation.z = 1
00403         control.orientation.w = 1
00404         control.name = 'rotate_y'
00405         control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00406         if fixed:
00407             control.orientation_mode = InteractiveMarkerControl.FIXED
00408         int_marker.controls.append(control)
00409 
00410         control = InteractiveMarkerControl()
00411         control.orientation.x = 0
00412         control.orientation.y = 1
00413         control.orientation.z = 0
00414         control.orientation.w = 1
00415         control.name = 'rotate_z'
00416         control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00417         if fixed:
00418             control.orientation_mode = InteractiveMarkerControl.FIXED
00419         int_marker.controls.append(control)
00420 
00421     return int_marker
00422     
00423 
00424 
00425 # initial_position - 3x1 np matrix
00426 # pose - geometry_msgs/PointStamped
00427 # scale, color, mtype -- see make_marker.
00428 def make_6dof_marker(fixed, ps, scale, color, mtype):
00429     return make_marker_flexible(fixed, ps, scale, color, mtype,
00430                                 ignore_rotation = False)
00431 
00432 def make_marker_flexible(fixed, ps, scale, color, mtype,
00433                          ignore_rotation, ignore_x=False,
00434                          ignore_y=False, ignore_z=False):
00435     int_marker = InteractiveMarker()
00436     int_marker.header.frame_id = ps.header.frame_id
00437     int_marker.pose.position.x = ps.point.x
00438     int_marker.pose.position.y = ps.point.y
00439     int_marker.pose.position.z = ps.point.z
00440     int_marker.scale = scale
00441 
00442     int_marker.name = 'simple_6dof'
00443     int_marker.description = ''
00444 
00445     # insert a marker
00446     control =  InteractiveMarkerControl()
00447     control.always_visible = True
00448     control.markers.append(make_marker(scale, color, mtype))
00449     int_marker.controls.append(control)
00450 
00451     if fixed:
00452         int_marker.name += '_fixed'
00453         int_marker.description += '\n(fixed orientation)'
00454 
00455     if not ignore_x:
00456         control = InteractiveMarkerControl()
00457         control.orientation.x = 1
00458         control.orientation.y = 0
00459         control.orientation.z = 0
00460         control.orientation.w = 1
00461         control.name = 'move_x'
00462         control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00463         if fixed:
00464             control.orientation_mode = InteractiveMarkerControl.FIXED
00465         int_marker.controls.append(control)
00466 
00467     if not ignore_y:
00468         control = InteractiveMarkerControl()
00469         control.orientation.x = 0
00470         control.orientation.y = 0
00471         control.orientation.z = 1
00472         control.orientation.w = 1
00473         control.name = 'move_y'
00474         control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00475         if fixed:
00476             control.orientation_mode = InteractiveMarkerControl.FIXED
00477         int_marker.controls.append(control)
00478 
00479     if not ignore_z:
00480         control = InteractiveMarkerControl()
00481         control.orientation.x = 0
00482         control.orientation.y = 1
00483         control.orientation.z = 0
00484         control.orientation.w = 1
00485         control.name = 'move_z'
00486         control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00487         if fixed:
00488             control.orientation_mode = InteractiveMarkerControl.FIXED
00489         int_marker.controls.append(control)
00490 
00491     if not ignore_rotation:
00492         control = InteractiveMarkerControl()
00493         control.orientation.x = 1
00494         control.orientation.y = 0
00495         control.orientation.z = 0
00496         control.orientation.w = 1
00497         control.name = 'rotate_x'
00498         control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00499         if fixed:
00500             control.orientation_mode = InteractiveMarkerControl.FIXED
00501         int_marker.controls.append(control)
00502 
00503         control = InteractiveMarkerControl()
00504         control.orientation.x = 0
00505         control.orientation.y = 0
00506         control.orientation.z = 1
00507         control.orientation.w = 1
00508         control.name = 'rotate_y'
00509         control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00510         if fixed:
00511             control.orientation_mode = InteractiveMarkerControl.FIXED
00512         int_marker.controls.append(control)
00513 
00514         control = InteractiveMarkerControl()
00515         control.orientation.x = 0
00516         control.orientation.y = 1
00517         control.orientation.z = 0
00518         control.orientation.w = 1
00519         control.name = 'rotate_z'
00520         control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00521         if fixed:
00522             control.orientation_mode = InteractiveMarkerControl.FIXED
00523         int_marker.controls.append(control)
00524 
00525     return int_marker
00526 
00527 
00528 # add this menu handler to the Interactive Marker.
00529 # server - InteractiveMarkerServer
00530 def add_menu_handler(int_marker, menu_handler, server):
00531     control = InteractiveMarkerControl()
00532     control.interaction_mode = InteractiveMarkerControl.MENU
00533     control.description="Options"
00534     control.name = "menu_only_control"
00535     int_marker.controls.append(control)
00536     menu_handler.apply(server, int_marker.name)
00537     server.applyChanges()
00538 
00539 
00540