testarmik5d.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 from __future__ import with_statement # for python 2.5
00003 __author__ = 'Rosen Diankov'
00004 __license__ = 'Apache License, Version 2.0'
00005 
00006 import roslib; roslib.load_manifest('orrosplanning')
00007 import rospy, time
00008 import orrosplanning.srv
00009 import geometry_msgs.msg
00010 import kinematics_msgs.srv
00011 from numpy import *
00012 from arm_navigation_msgs.msg import ArmNavigationErrorCodes
00013 
00014 if __name__=='__main__':
00015     rospy.init_node('armik_test')
00016     rospy.wait_for_service('IK')
00017     IKFn = rospy.ServiceProxy('IK',orrosplanning.srv.IK)
00018     req = orrosplanning.srv.IKRequest()
00019     req.pose_stamped.header.frame_id = 'Base'
00020     req.manip_name = 'arm'
00021     req.iktype = 'TranslationDirection5D'
00022     req.filteroptions = orrosplanning.srv.IKRequest.RETURN_CLOSEST_SOLUTION
00023     while True:
00024         randomquat = random.rand(4)
00025         randomquat /= linalg.norm(randomquat)
00026         req.pose_stamped.pose.orientation = geometry_msgs.msg.Quaternion(*list(randomquat))
00027         coords = random.rand(3)-0.5
00028         req.pose_stamped.pose.position = geometry_msgs.msg.Point(*list(coords))
00029         res=IKFn(req)
00030         if res is not None and res.error_code.val == ArmNavigationErrorCodes.SUCCESS:
00031             print 'found solution!'
00032             print res
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


orrosplanning
Author(s): Rosen Diankov (rosen.diankov@gmail.com)
autogenerated on Sat Mar 23 2013 22:32:59