def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description="Views a calibration pattern from multiple locations.") OpenRAVEGlobalArguments.addOptions(parser) parser.add_option( "--scene", action="store", type="string", dest="scene", default="data/pa10calib.env.xml", help="Scene file to load (default=%default)", ) parser.add_option( "--sensorname", action="store", type="string", dest="sensorname", default=None, help="Name of the sensor whose views to generate (default is first sensor on robot)", ) parser.add_option( "--sensorrobot", action="store", type="string", dest="sensorrobot", default=None, help="Name of the robot the sensor is attached to (default=%default)", ) parser.add_option( "--norandomize", action="store_false", dest="randomize", default=True, help="If set, will not randomize the bodies and robot position in the scene.", ) parser.add_option( "--novisibility", action="store_false", dest="usevisibility", default=True, help="If set, will not perform any visibility searching.", ) parser.add_option( "--posedist", action="store", type="float", dest="posedist", default=0.05, help="An average distance between gathered poses. The smaller the value, the more poses robot will gather close to each other", ) (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options, defaultviewer=True) main(env, options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description='Example shows how to query collision detection information using openravepy') OpenRAVEGlobalArguments.addOptions(parser) (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description="test physics") OpenRAVEGlobalArguments.addOptions(parser) (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ # set up planning environment parser = OptionParser(description='Find the transform that moves the hand to target') OpenRAVEGlobalArguments.addOptions(parser) (options, leftargs) = parser.parse_args(args=args) # use default options env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) # the special setup for openrave tutorial main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description='Shows how to use different IK solutions for arms with few joints.') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--scene',action="store",type='string',dest='scene',default='data/pr2wam_test1.env.xml', help='Scene file to load (default=%default)') (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description='Explicitly specify goals to get a simple navigation and manipulation demo.', usage='openrave.py --example hanoi [options]') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--planner',action="store",type='string',dest='planner',default=None, help='the planner to use') (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description="test physics diff drive controller") parser.add_option('--scene',action="store",type='string',dest='scene',default='data/diffdrive_sample.env.xml', help='Scene file to load (default=%default)') OpenRAVEGlobalArguments.addOptions(parser) (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description='Shows how to attach a callback to a viewer to perform functions.') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--scene', action="store",type='string',dest='scene',default='data/lab1.env.xml', help='OpenRAVE scene to load') (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description='RRT motion planning with constraints on the robot end effector.') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--scene', action="store",type='string',dest='scene',default='data/lab1.env.xml', help='Scene file to load (default=%default)') (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description='Computes if an object is visibile inside the robot cameras.') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--scene', action="store",type='string',dest='scene',default='data/testwamcamera.env.xml', help='Scene file to load (default=%default)') (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description='Displays all images of all camera sensors attached to a robot.') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--scene', action="store",type='string',dest='scene',default='data/testwamcamera.env.xml', help='OpenRAVE scene to load') (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description='Example showing how to compute a valid grasp as fast as possible without computing a grasp set, this is used when the target objects change frequently.') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--scene', action="store",type='string',dest='scene',default='data/wamtest1.env.xml', help='Scene file to load (default=%default)') parser.add_option('--manipname', action="store",type='string',dest='manipname',default=None, help='Choose the manipulator to perform the grasping for') (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description='Builds the convex decomposition of the robot and plots all the points that are tested inside of it.') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--target', action="store",type='string',dest='target',default='robots/barrettwam.robot.xml', help='Target body to load (default=%default)') parser.add_option('--samplingdelta', action="store",type='float',dest='samplingdelta',default=None, help='The sampling rate for the robot (default=%default)') (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description='Visibility Planning Module.') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--scene',action="store",type='string',dest='scene',default='data/pa10grasp.env.xml', help='openrave scene to load') parser.add_option('--nocameraview',action="store_false",dest='usecameraview',default=True, help='If set, will not open any camera views') (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description='Manipulation planning example solving the hanoi problem.', usage='openrave.py --example hanoi [options]') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--scene',action="store",type='string',dest='scene',default='data/hanoi_complex2.env.xml', help='Scene file to load (default=%default)') parser.add_option('--planner',action="store",type='string',dest='planner',default=None, help='the planner to use') (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description='Shows how to use the grasping.GraspingModel to compute valid grasps for manipulation.') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--scene', action="store",type='string',dest='scene',default='data/wamtest1.env.xml', help='Scene file to load (default=%default)') parser.add_option('--manipname', action="store",type='string',dest='manipname',default=None, help='Choose the manipulator to perform the grasping for') (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description="test physics") OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--scene',action="store",type='string',dest='scene',default='data/hanoi.env.xml', help='Scene file to load (default=%default)') parser.add_option('--timestep',action="store",type='float',dest='timestep',default=0.001, help='The physics simulation time step size (default=%default)') (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description='Simple cube assembly task using grasp sets.') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--scene', action="store",type='string',dest='scene',default='data/hironxtable.env.xml', help='Scene file to load (default=%default)') parser.add_option('--manipname', action="store",type='string',dest='manipname',default='leftarm_torso', help='The manipulator to use') (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) main(env,options)
[docs]def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description='Move base where the robot can perform target grasp using inversereachability database.') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--robot',action="store",type='string',dest='robot',default='robots/pr2-beta-static.zae', help='Robot filename to use (default=%default)') parser.add_option('--manipname',action="store",type='string',dest='manipname',default=None, help='name of manipulator to use (default=%default)') parser.add_option('--target',action="store",type='string',dest='target',default='data/mug2.kinbody.xml', help='filename of the target to use (default=%default)') (options, leftargs) = parser.parse_args(args=args) # use default options env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) # the special setup for openrave tutorial main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description='Shows how choose IK solutions so that move hand straight can move without discontinuities.') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--scene', action="store",type='string',dest='scene',default='data/puma_tabletop.env.xml', help='Scene file to load (default=%default)') parser.add_option('--manipname', action="store",type='string',dest='manipname',default=None, help='Choose the manipulator to perform movement for') (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description='Move base where the robot can perform target grasp using inversereachability database.') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--robot',action="store",type='string',dest='robot',default='robots/pr2-beta-static.zae', help='Robot filename to use (default=%default)') parser.add_option('--manipname',action="store",type='string',dest='manipname',default=None, help='name of manipulator to use (default=%default)') parser.add_option('--target',action="store",type='string',dest='target',default='data/mug2.kinbody.xml', help='filename of the target to use (default=%default)') (options, leftargs) = parser.parse_args(args=args) # use default options env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) # the special setup for openrave tutorial main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description='Shows how to use different IK solutions for arms with few joints.') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--scene',action="store",type='string',dest='scene',default='data/katanatable.env.xml', help='Scene file to load (default=%default)') parser.add_option('--manipname',action="store",type='string',dest='manipname',default='arm', help='name of manipulator to use (default=%default)') parser.add_option('--withlocal',action="store_true",dest='withlocal',default=False, help='If set, will use the TranslationLocalGlobal6D type to further specify the target point in the manipulator coordinate system') (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description='Shows how to generate a 6D inverse kinematics solver and use it for getting all solutions.') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--scene',action="store",type='string',dest='scene',default='data/lab1.env.xml', help='Scene file to load (default=%default)') parser.add_option('--transparency',action="store",type='float',dest='transparency',default=0.8, help='Transparency for every robot (default=%default)') parser.add_option('--maxnumber',action="store",type='int',dest='maxnumber',default=10, help='Max number of robots to render (default=%default)') parser.add_option('--manipname',action="store",type='string',dest='manipname',default=None, help='name of manipulator to use (default=%default)') (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description='Autonomous grasp and manipulation planning example.') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--scene', action="store",type='string',dest='scene',default='/home/user/testopenrave/data/mytest.env.xml', help='Scene file to load (default=%default)') parser.add_option('--nodestinations', action='store_true',dest='nodestinations',default=False, help='If set, will plan without destinations.') parser.add_option('--norandomize', action='store_false',dest='randomize',default=True, help='If set, will not randomize the bodies and robot position in the scene.') parser.add_option('--planner',action="store",type='string',dest='planner',default=None, help='the planner to use') (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) main(env,options)
def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser( description="Schunk Manipulation planning example\nFor a dual arm robot with Schunk LWA3 arms, plan trajectories for grasping an object and manipulating it on a shelf." ) OpenRAVEGlobalArguments.addOptions(parser) parser.add_option( "--scene", action="store", type="string", dest="scene", default="data/dualarmmanipulation.env.xml", help="Scene file to load", ) (options, leftargs) = parser.parse_args(args=args) env = OpenRAVEGlobalArguments.parseAndCreate(options, defaultviewer=True) main(env, options)
def spin(self): if True: # Create a world object world = ode.World() # world.setGravity( (0,0,-3.81) ) world.setGravity( (0,0,0) ) CFM = world.getCFM() ERP = world.getERP() print "CFM: %s ERP: %s"%(CFM, ERP) # world.setCFM(0.001) # print "CFM: %s ERP: %s"%(CFM, ERP) m_id = 0 self.pub_marker.eraseMarkers(0,3000, frame_id='world') # # Init Openrave # parser = OptionParser(description='Openrave Velma interface') OpenRAVEGlobalArguments.addOptions(parser) (options, leftargs) = parser.parse_args() self.env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) # self.openrave_robot = self.env.ReadRobotXMLFile('robots/barretthand_ros.robot.xml') mimic_joints = [ ("right_HandFingerTwoKnuckleOneJoint", "right_HandFingerOneKnuckleOneJoint*1.0", "|right_HandFingerOneKnuckleOneJoint 1.0", ""), ("right_HandFingerOneKnuckleThreeJoint", "right_HandFingerOneKnuckleTwoJoint*0.33333", "|right_HandFingerOneKnuckleTwoJoint 0.33333", ""), ("right_HandFingerTwoKnuckleThreeJoint", "right_HandFingerTwoKnuckleTwoJoint*0.33333", "|right_HandFingerTwoKnuckleTwoJoint 0.33333", ""), ("right_HandFingerThreeKnuckleThreeJoint", "right_HandFingerThreeKnuckleTwoJoint*0.33333", "|right_HandFingerThreeKnuckleTwoJoint 0.33333", ""), ] rospack = rospkg.RosPack() self.urdf_module = RaveCreateModule(self.env, 'urdf') xacro_uri = rospack.get_path('barrett_hand_defs') + '/robots/barrett_hand.urdf.xml' urdf_uri = '/tmp/barrett_hand.urdf' srdf_uri = rospack.get_path('barrett_hand_defs') + '/robots/barrett_hand.srdf' arg1 = "collision_model_full:=true" arg2 = "collision_model_simplified:=false" arg3 = "collision_model_enlargement:=0.0" arg4 = "collision_model_no_hands:=false" subprocess.call(["rosrun", "xacro", "xacro", "-o", urdf_uri, xacro_uri, arg1, arg2, arg3, arg4]) robot_name = self.urdf_module.SendCommand('load ' + urdf_uri + ' ' + srdf_uri ) print "robot name: " + robot_name self.openrave_robot = self.env.GetRobot(robot_name) self.env.Remove(self.openrave_robot) for mimic in mimic_joints: mj = self.openrave_robot.GetJoint(mimic[0]) mj.SetMimicEquations(0, mimic[1], mimic[2], mimic[3]) self.openrave_robot.GetActiveManipulator().SetChuckingDirection([0,0,0,0])#directions) self.env.Add(self.openrave_robot) self.openrave_robot = self.env.GetRobot(robot_name) # print "robots: " # print self.env.GetRobots() joint_names = [] print "active joints:" for j in self.openrave_robot.GetJoints(): joint_names.append(j.GetName()) print j print "passive joints:" for j in self.openrave_robot.GetPassiveJoints(): joint_names.append(j.GetName()) print j # ODE does not support distance measure self.env.GetCollisionChecker().SetCollisionOptions(CollisionOptions.Contacts) self.env.Add(self.openrave_robot) vertices, faces = surfaceutils.readStl(rospack.get_path('velma_scripts') + "/data/meshes/klucz_gerda_ascii.stl", scale=1.0) # vertices, faces = surfaceutils.readStl("klucz_gerda_ascii.stl", scale=1.0) self.addTrimesh("object", vertices, faces) # self.addBox("object", 0.2,0.06,0.06) # self.addSphere("object", 0.15) # vertices, faces = self.getMesh("object") # # definition of the expected external wrenches # ext_wrenches = [] # origin of the external wrench (the end point of the key) wr_orig = PyKDL.Vector(0.039, 0.0, 0.0) for i in range(8): # expected force at the end point force = PyKDL.Frame(PyKDL.Rotation.RotX(float(i)/8.0 * 2.0 * math.pi)) * PyKDL.Vector(0,1,0) # expected torque at the com torque = wr_orig * force ext_wrenches.append(PyKDL.Wrench(force, torque)) # expected force at the end point force = PyKDL.Frame(PyKDL.Rotation.RotX(float(i)/8.0 * 2.0 * math.pi)) * PyKDL.Vector(-1,1,0) # expected torque at the com torque = wr_orig * force ext_wrenches.append(PyKDL.Wrench(force, torque)) # # definition of the grasps # grasp_id = 0 if grasp_id == 0: grasp_direction = [0, 1, -1, 1] # spread, f1, f3, f2 grasp_initial_configuration = [60.0/180.0*math.pi, None, None, None] self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.122103662206, -0.124395758838, -0.702726011729, 0.689777190329), PyKDL.Vector(-0.00115787237883, -0.0194999426603, 0.458197712898)) # self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.174202588426, -0.177472708083, -0.691231954612, 0.678495061771), PyKDL.Vector(0.0, -0.0213436260819, 0.459123969078)) elif grasp_id == 1: grasp_direction = [0, 1, -1, 1] # spread, f1, f3, f2 grasp_initial_configuration = [90.0/180.0*math.pi, None, None, None] self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0187387771868, -0.708157209758, -0.0317875569224, 0.705090018033), PyKDL.Vector(4.65661287308e-10, 0.00145332887769, 0.472836345434)) elif grasp_id == 2: grasp_direction = [0, 1, 0, 0] # spread, f1, f3, f2 grasp_initial_configuration = [90.0/180.0*math.pi, None, 90.0/180.0*math.pi, 0] self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0187387763947, -0.708157179826, -0.0317875555789, 0.705089928626), PyKDL.Vector(0.0143095180392, 0.00145332887769, 0.483659058809)) elif grasp_id == 3: grasp_direction = [0, 1, 1, 1] # spread, f1, f3, f2 grasp_initial_configuration = [90.0/180.0*math.pi, None, None, None] self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.00518634245761, -0.706548316769, -0.0182458505507, 0.707410947861), PyKDL.Vector(0.000126354629174, -0.00217361748219, 0.47637796402)) elif grasp_id == 4: grasp_direction = [0, 0, 1, 0] # spread, f1, f3, f2 grasp_initial_configuration = [90.0/180.0*math.pi, 100.0/180.0*math.pi, None, 100.0/180.0*math.pi] self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.153445252933, -0.161230275653, 0.681741576082, 0.696913201022), PyKDL.Vector(0.000126355327666, 0.00152841210365, 0.466048002243)) elif grasp_id == 5: grasp_direction = [0, 0, 1, 0] # spread, f1, f3, f2 grasp_initial_configuration = [100.0/180.0*math.pi, 101.5/180.0*math.pi, None, 101.5/180.0*math.pi] self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.155488650062, -0.159260521271, 0.690572597636, 0.688163302213), PyKDL.Vector(-0.000278688268736, 0.00575117766857, 0.461560428143)) elif grasp_id == 6: grasp_direction = [0, 1, -1, 1] # spread, f1, f3, f2 grasp_initial_configuration = [90.0/180.0*math.pi, None, 0, 0] self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.512641041738, -0.485843507183, -0.514213889193, 0.48655882699), PyKDL.Vector(-0.000278423947748, -0.00292747467756, 0.445628076792)) else: print "ERROR: unknown grasp_id: %s"%(grasp_id) exit(0) self.updatePose("object", self.T_W_O) with open('barret_hand_openrave2ros_joint_map2.txt', 'r') as f: lines = f.readlines() joint_map = {} for line in lines: line_s = line.split() if len(line_s) == 2: joint_map[line_s[0]] = line_s[1] elif len(line_s) != 1: print "error in joint map file" exit(0) print joint_map self.pub_js = rospy.Publisher("/joint_states", JointState) if True: def getDirectionIndex(n): min_angle = -45.0/180.0*math.pi angle_range = 90.0/180.0*math.pi angles_count = 5 angles_count2 = angles_count * angles_count max_indices = angles_count2 * 6 if abs(n.x()) > abs(n.y()) and abs(n.x()) > abs(n.z()): if n.x() > 0: sec = 0 a1 = math.atan2(n.y(), n.x()) a2 = math.atan2(n.z(), n.x()) else: sec = 1 a1 = math.atan2(n.y(), -n.x()) a2 = math.atan2(n.z(), -n.x()) elif abs(n.y()) > abs(n.x()) and abs(n.y()) > abs(n.z()): if n.y() > 0: sec = 2 a1 = math.atan2(n.x(), n.y()) a2 = math.atan2(n.z(), n.y()) else: sec = 3 a1 = math.atan2(n.x(), -n.y()) a2 = math.atan2(n.z(), -n.y()) else: if n.z() > 0: sec = 4 a1 = math.atan2(n.x(), n.z()) a2 = math.atan2(n.y(), n.z()) else: sec = 5 a1 = math.atan2(n.x(), -n.z()) a2 = math.atan2(n.y(), -n.z()) a1i = int(angles_count*(a1-min_angle)/angle_range) a2i = int(angles_count*(a2-min_angle)/angle_range) if a1i < 0: print sec, a1i, a2i a1i = 0 if a1i >= angles_count: # print sec, a1i, a2i a1i = angles_count-1 if a2i < 0: print sec, a1i, a2i a2i = 0 if a2i >= angles_count: print sec, a1i, a2i a2i = angles_count-1 return sec * angles_count2 + a1i * angles_count + a2i # generate a dictionary of indexed directions normals_sphere_indexed_dir = velmautils.generateNormalsSphere(3.0/180.0*math.pi) print len(normals_sphere_indexed_dir) indices_directions = {} for n in normals_sphere_indexed_dir: index = getDirectionIndex(n) if not index in indices_directions: indices_directions[index] = [n] else: indices_directions[index].append(n) for index in indices_directions: n_mean = PyKDL.Vector() for n in indices_directions[index]: n_mean += n n_mean.Normalize() indices_directions[index] = n_mean # test direction indexing if False: normals_sphere = velmautils.generateNormalsSphere(3.0/180.0*math.pi) print len(normals_sphere) m_id = 0 for current_index in range(5*5*6): count = 0 r = random.random() g = random.random() b = random.random() for n in normals_sphere: sec = -1 a1 = None a2 = None index = getDirectionIndex(n) if index == current_index: m_id = self.pub_marker.publishSinglePointMarker(n*0.1, m_id, r=r, g=g, b=b, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.007, 0.007, 0.007), T=None) count += 1 print current_index, count for index in indices_directions: m_id = self.pub_marker.publishSinglePointMarker(indices_directions[index]*0.12, m_id, r=1, g=1, b=1, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.012, 0.012, 0.012), T=None) exit(0) # # PyODE test # if True: fixed_joints_names_for_fixed_DOF = [ ["right_HandFingerOneKnuckleOneJoint", "right_HandFingerTwoKnuckleOneJoint"], # spread ["right_HandFingerOneKnuckleTwoJoint", "right_HandFingerOneKnuckleThreeJoint"], # f1 ["right_HandFingerThreeKnuckleTwoJoint", "right_HandFingerThreeKnuckleThreeJoint"], # f3 ["right_HandFingerTwoKnuckleTwoJoint", "right_HandFingerTwoKnuckleThreeJoint"], # f2 ] coupled_joint_names_for_fingers = ["right_HandFingerOneKnuckleThreeJoint", "right_HandFingerTwoKnuckleThreeJoint", "right_HandFingerThreeKnuckleThreeJoint"] actuated_joints_for_DOF = [ ["right_HandFingerOneKnuckleOneJoint", "right_HandFingerTwoKnuckleOneJoint"], # spread ["right_HandFingerOneKnuckleTwoJoint"], # f1 ["right_HandFingerThreeKnuckleTwoJoint"], # f3 ["right_HandFingerTwoKnuckleTwoJoint"]] # f2 ignored_links = ["world", "world_link"] self.run_ode_simulation = False self.insert6DofGlobalMarker(self.T_W_O) print "grasp_direction = [%s, %s, %s, %s] # spread, f1, f3, f2"%(grasp_direction[0], grasp_direction[1], grasp_direction[2], grasp_direction[3]) print "grasp_initial_configuration = [%s, %s, %s, %s]"%(grasp_initial_configuration[0], grasp_initial_configuration[1], grasp_initial_configuration[2], grasp_initial_configuration[3]) print "grasp_final_configuration = %s"%(self.openrave_robot.GetDOFValues()) rot_q = self.T_W_O.M.GetQuaternion() print "self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(%s, %s, %s, %s), PyKDL.Vector(%s, %s, %s))"%(rot_q[0], rot_q[1], rot_q[2], rot_q[3], self.T_W_O.p.x(), self.T_W_O.p.y(), self.T_W_O.p.z()) self.erase6DofMarker() raw_input("Press ENTER to continue...") # reset the gripper in Openrave self.openrave_robot.SetDOFValues([0,0,0,0]) # read the position of all links grasp_links_poses = {} for link in self.openrave_robot.GetLinks(): grasp_links_poses[link.GetName()] = self.OpenraveToKDL(link.GetTransform()) # print "obtained the gripper configuration for the grasp:" # print finalconfig[0] # # simulation in ODE # # Create a world object world = ode.World() # world.setGravity( (0,0,-3.81) ) world.setGravity( (0,0,0) ) CFM = world.getCFM() ERP = world.getERP() print "CFM: %s ERP: %s"%(CFM, ERP) # world.setCFM(0.001) # print "CFM: %s ERP: %s"%(CFM, ERP) self.space = ode.Space() # Create a body inside the world body = ode.Body(world) M = ode.Mass() M.setCylinderTotal(0.02, 1, 0.005, 0.09) body.setMass(M) ode_mesh = ode.TriMeshData() ode_mesh.build(vertices, faces) geom = ode.GeomTriMesh(ode_mesh, self.space) geom.name = "object" geom.setBody(body) self.setOdeBodyPose(geom, self.T_W_O) ode_gripper_geoms = {} for link in self.openrave_robot.GetLinks(): link_name = link.GetName() if link_name in ignored_links: print "ignoring: %s"%(link_name) continue print "adding: %s"%(link_name) ode_mesh_link = None body_link = None T_W_L = self.OpenraveToKDL(link.GetTransform()) col = link.GetCollisionData() vertices = col.vertices faces = col.indices ode_mesh_link = ode.TriMeshData() ode_mesh_link.build(vertices, faces) ode_gripper_geoms[link_name] = ode.GeomTriMesh(ode_mesh_link, self.space) if True: body_link = ode.Body(world) M_link = ode.Mass() M_link.setCylinderTotal(0.05, 1, 0.01, 0.09) body_link.setMass(M_link) ode_gripper_geoms[link_name].setBody(body_link) ode_gripper_geoms[link_name].name = link.GetName() self.setOdeBodyPose(body_link, T_W_L) actuated_joint_names = [] for dof in range(4): for joint_name in actuated_joints_for_DOF[dof]: actuated_joint_names.append(joint_name) ode_gripper_joints = {} for joint_name in joint_names: joint = self.openrave_robot.GetJoint(joint_name) link_parent = joint.GetHierarchyParentLink().GetName() link_child = joint.GetHierarchyChildLink().GetName() if link_parent in ode_gripper_geoms and link_child in ode_gripper_geoms: axis = joint.GetAxis() anchor = joint.GetAnchor() if joint_name in actuated_joint_names: ode_gripper_joints[joint_name] = ode.AMotor(world) ode_gripper_joints[joint_name].attach(ode_gripper_geoms[link_parent].getBody(), ode_gripper_geoms[link_child].getBody()) ode_gripper_joints[joint_name].setMode(ode.AMotorUser)#Euler) # ode_gripper_joints[joint_name].setNumAxes(3) ode_gripper_joints[joint_name].setAxis(0,1,axis) # ode_gripper_joints[joint_name].setAxis(1,1,axis) # ode_gripper_joints[joint_name].setAxis(2,1,axis) ode_gripper_joints[joint_name].setParam(ode.ParamFMax, 10.0) # ode_gripper_joints[joint_name].setParam(ode.ParamFMax2, 10.0) # ode_gripper_joints[joint_name].setParam(ode.ParamFMax3, 10.0) else: ode_gripper_joints[joint_name] = ode.HingeJoint(world) ode_gripper_joints[joint_name].attach(ode_gripper_geoms[link_parent].getBody(), ode_gripper_geoms[link_child].getBody()) ode_gripper_joints[joint_name].setAxis(-axis) ode_gripper_joints[joint_name].setAnchor(anchor) # ode_gripper_joints[joint_name] = ode.HingeJoint(world) # ode_gripper_joints[joint_name].attach(ode_gripper_geoms[link_parent].getBody(), ode_gripper_geoms[link_child].getBody()) # ode_gripper_joints[joint_name].setAxis(-axis) # ode_gripper_joints[joint_name].setAnchor(anchor) # if joint_name in actuated_joint_names: # ode_gripper_joints[joint_name].setParam(ode.ParamFMax, 10.0) limits = joint.GetLimits() value = joint.GetValue(0) # print joint_name # print "limits: %s %s"%(limits[0], limits[1]) # print "axis: %s"%(axis) # print "anchor: %s"%(anchor) # print "value: %s"%(value) lim = [limits[0], limits[1]] if limits[0] <= -math.pi: # print "lower joint limit %s <= -PI, setting to -PI+0.01"%(limits[0]) lim[0] = -math.pi + 0.01 if limits[1] >= math.pi: # print "upper joint limit %s >= PI, setting to PI-0.01"%(limits[1]) lim[1] = math.pi - 0.01 ode_gripper_joints[joint_name].setParam(ode.ParamLoStop, lim[0]) ode_gripper_joints[joint_name].setParam(ode.ParamHiStop, lim[1]) # ode_gripper_joints[joint_name].setParam(ode.ParamFudgeFactor, 0.5) ode_fixed_joint = ode.FixedJoint(world) ode_fixed_joint.attach(None, ode_gripper_geoms["right_HandPalmLink"].getBody()) ode_fixed_joint.setFixed() # # set the poses of all links as for the grasp # for link_name in grasp_links_poses: if link_name in ignored_links: continue ode_body = ode_gripper_geoms[link_name].getBody() T_W_L = grasp_links_poses[link_name] self.setOdeBodyPose(ode_body, T_W_L) fixed_joint_names = [] fixed_joint_names += coupled_joint_names_for_fingers for dof in range(4): if grasp_direction[dof] == 0.0: for joint_name in fixed_joints_names_for_fixed_DOF[dof]: if not joint_name in fixed_joint_names: fixed_joint_names.append(joint_name) # # change all coupled joints to fixed joints # fixed_joints = {} for joint_name in fixed_joint_names: # save the bodies attached body0 = ode_gripper_joints[joint_name].getBody(0) body1 = ode_gripper_joints[joint_name].getBody(1) # save the joint angle if joint_name in actuated_joint_names: angle = ode_gripper_joints[joint_name].getAngle(0) else: angle = ode_gripper_joints[joint_name].getAngle() # detach the joint ode_gripper_joints[joint_name].attach(None, None) del ode_gripper_joints[joint_name] fixed_joints[joint_name] = [ode.FixedJoint(world), angle] fixed_joints[joint_name][0].attach(body0, body1) fixed_joints[joint_name][0].setFixed() # change all actuated joints to motor joints # actuated_joint_names = [] # for dof in range(4): # for joint_name in actuated_joints_for_DOF[dof]: # actuated_joint_names.append(joint_name) # for joint_name in actuated_joint_names: # A joint group for the contact joints that are generated whenever # two bodies collide contactgroup = ode.JointGroup() print "ode_gripper_geoms:" print ode_gripper_geoms initial_T_W_O = self.T_W_O # Do the simulation... dt = 0.001 total_time = 0.0 failure = False # # PID # Kc = 1.0 # Ti = 1000000000000.0 KcTi = 0.0 Td = 0.0 Ts = 0.001 pos_d = 1.0 e0 = 0.0 e1 = 0.0 e2 = 0.0 u = 0.0 u_max = 1.0 while total_time < 5.0 and not rospy.is_shutdown(): # # ODE simulation # joint = ode_gripper_joints["right_HandFingerOneKnuckleTwoJoint"] e2 = e1 e1 = e0 e0 = pos_d - joint.getAngle(0) u = u + Kc * (e0 - e1) + KcTi * Ts * e0 + Kc * Td / Ts * (e0 - 2.0 * e1 + e2) if u > u_max: u = u_max if u < -u_max: u = -u_max joint.setParam(ode.ParamFMax, 10000.0) joint.setParam(ode.ParamVel, 1.1) # joint.setParam(ode.ParamVel2, 1.1) # joint.setParam(ode.ParamVel3, 1.1) print joint.getAngle(0), " ", u #, " ", joint.getAngleRate(0) # print u # joint.addTorque(u) # for dof in range(4): # for joint_name in actuated_joints_for_DOF[dof]: # if joint_name in ode_gripper_joints: # ode_gripper_joints[joint_name].addTorque(1*grasp_direction[dof]) self.grasp_contacts = [] self.space.collide((world,contactgroup), self.near_callback) world.step(dt) total_time += dt contactgroup.empty() # # ROS interface # old_m_id = m_id m_id = 0 # publish frames from ODE if False: for link_name in ode_gripper_geoms: link_body = ode_gripper_geoms[link_name].getBody() if link_body == None: link_body = ode_gripper_geoms[link_name] T_W_Lsim = self.getOdeBodyPose(link_body) m_id = self.pub_marker.publishFrameMarker(T_W_Lsim, m_id, scale=0.05, frame='world', namespace='default') # publish the mesh of the object T_W_Osim = self.getOdeBodyPose(body) m_id = self.pub_marker.publishConstantMeshMarker("package://velma_scripts/data/meshes/klucz_gerda_binary.stl", m_id, r=1, g=0, b=0, scale=1.0, frame_id='world', namespace='default', T=T_W_Osim) # update the gripper visualization in ros js = JointState() js.header.stamp = rospy.Time.now() for jn in joint_map: ros_joint_name = joint_map[jn] js.name.append(ros_joint_name) if jn in ode_gripper_joints: if jn in actuated_joint_names: js.position.append( ode_gripper_joints[jn].getAngle(0) ) else: js.position.append( ode_gripper_joints[jn].getAngle() ) elif jn in fixed_joints: js.position.append(fixed_joints[jn][1]) else: js.position.append(0) self.pub_js.publish(js) # draw contacts for c in self.grasp_contacts: cc = PyKDL.Vector(c[0], c[1], c[2]) cn = PyKDL.Vector(c[3], c[4], c[5]) m_id = self.pub_marker.publishVectorMarker(cc, cc+cn*0.04, m_id, 1, 0, 0, frame='world', namespace='default', scale=0.003) if m_id < old_m_id: self.pub_marker.eraseMarkers(m_id,old_m_id+1, frame_id='world') diff_T_W_O = PyKDL.diff(initial_T_W_O, T_W_Osim) # if diff_T_W_O.vel.Norm() > 0.02 or diff_T_W_O.rot.Norm() > 30.0/180.0*math.pi: # print "the object moved" # print diff_T_W_O # failure = True # break rospy.sleep(0.01) if not failure: T_O_Wsim = T_W_Osim.Inverse() TR_O_Wsim = PyKDL.Frame(T_O_Wsim.M) contacts = [] for c in self.grasp_contacts: cc_W = PyKDL.Vector(c[0], c[1], c[2]) cn_W = PyKDL.Vector(c[3], c[4], c[5]) cc_O = T_O_Wsim * cc_W cn_O = TR_O_Wsim * cn_W contacts.append([cc_O[0], cc_O[1], cc_O[2], cn_O[0], cn_O[1], cn_O[2]]) gws, contact_planes = self.generateGWS(contacts, 1.0) grasp_quality = None for wr in ext_wrenches: wr_qual = self.getQualityMeasure2(gws, wr) if grasp_quality == None or wr_qual < grasp_quality: grasp_quality = wr_qual grasp_quality_classic = self.getQualityMeasure(gws) print "grasp_quality_classic: %s grasp_quality: %s"%(grasp_quality_classic, grasp_quality) exit(0)
def spin(self): parser = OptionParser(description='Openrave Velma interface') OpenRAVEGlobalArguments.addOptions(parser) (options, leftargs) = parser.parse_args() self.env = OpenRAVEGlobalArguments.parseAndCreate(options)#,defaultviewer=True) self.openrave_robot = self.env.ReadRobotXMLFile('robots/barretthand_ros.robot.xml') link = self.openrave_robot.GetLink("right_HandFingerOneKnuckleThreeLink") col = link.GetCollisionData() vertices = col.vertices faces = col.indices print "sampling the surface..." points = surfaceutils.sampleMeshDetailedRays(vertices, faces, 0.001) print "surface has %s points"%(len(points)) # vertices, faces = surfaceutils.readStl("klucz_gerda_ascii.stl", scale=1.0) # points = surfaceutils.sampleMeshDetailedRays(vertices, faces, 0.002) print len(points) with open('points.txt', 'w') as f: for pt in points: p = pt.pos f.write(str(p.x()) + ' ' + str(p.y()) + ' ' + str(p.z()) + '\n') call(["touch","out_surf.off"]) call(["touch","out_axis.off"]) call(["/home/dseredyn/code/cocone/tightcocone/tcocone-linux", "-m", "points.txt", "out"]) calc_radius = False points_medial = [] with open('out_axis.off', 'r') as f: header = f.readline() header_s = header.split() p_count = int(header_s[1]) for i in range(0, p_count): line = f.readline() values = line.split() center = PyKDL.Vector(float(values[0]), float(values[1]), float(values[2])) if calc_radius: radius = None for pt in points: dist = (center-pt.pos).Norm() if radius == None or radius > dist: radius = dist points_medial.append([center, radius*2.0]) else: points_medial.append([center, 0.001]) print len(points_medial) # m_id = 0 # for p in points: # m_id = self.pub_marker.publishSinglePointMarker(PyKDL.Vector(p[0], p[1], p[2]), m_id, r=0, g=1, b=0, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.005, 0.005, 0.005), T=None) # m_id = 0 # for p in points_medial: # m_id = self.pub_marker.publishSinglePointMarker(p[0], m_id, r=1, g=0, b=0, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(p[1],p[1],p[1]), T=None) m_id = 0 m_id = self.pub_marker.publishMultiPointsMarkerWithSize(points_medial, m_id, r=1, g=0, b=0, namespace='default', frame_id='world', m_type=Marker.SPHERE, T=None) # m_id = self.pub_marker.publishMultiPointsMarker(points_medial, m_id, r=1, g=0, b=0, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(0.0005, 0.0005, 0.0005), T=None) rospy.sleep(1) exit(0)
dest='collision_map', default='/collision_map/collision_map', help= 'The collision map topic (maping_msgs/CollisionMap), by (default=%default)' ) parser.add_option( '--mapframe', action="store", type='string', dest='mapframe', default=None, help= 'The frame of the map used to position the robot. If --mapframe="" is specified, then nothing will be transformed with tf' ) (options, args) = parser.parse_args() env = OpenRAVEGlobalArguments.parseAndCreate(options, defaultviewer=True) RaveLoadPlugin( os.path.join(roslib.packages.get_pkg_dir('orrosplanning'), 'lib', 'liborrosplanning.so')) RaveLoadPlugin( os.path.join(roslib.packages.get_pkg_dir('openraveros'), 'lib', 'openraveros')) namespace = 'openrave' env.AddModule(RaveCreateModule(env, 'rosserver'), namespace) # load the orrosplanning plugin print 'initializing, please wait for ready signal...' try: rospy.init_node('ik_openrave', disable_signals=False) with env:
def spin(self): parser = OptionParser(description='Openrave Velma interface') OpenRAVEGlobalArguments.addOptions(parser) (options, leftargs) = parser.parse_args() self.env = OpenRAVEGlobalArguments.parseAndCreate( options) #,defaultviewer=True) self.openrave_robot = self.env.ReadRobotXMLFile( 'robots/barretthand_ros.robot.xml') link = self.openrave_robot.GetLink( "right_HandFingerOneKnuckleThreeLink") col = link.GetCollisionData() vertices = col.vertices faces = col.indices print "sampling the surface..." points = surfaceutils.sampleMeshDetailedRays(vertices, faces, 0.001) print "surface has %s points" % (len(points)) # vertices, faces = surfaceutils.readStl("klucz_gerda_ascii.stl", scale=1.0) # points = surfaceutils.sampleMeshDetailedRays(vertices, faces, 0.002) print len(points) with open('points.txt', 'w') as f: for pt in points: p = pt.pos f.write( str(p.x()) + ' ' + str(p.y()) + ' ' + str(p.z()) + '\n') call(["touch", "out_surf.off"]) call(["touch", "out_axis.off"]) call([ "/home/dseredyn/code/cocone/tightcocone/tcocone-linux", "-m", "points.txt", "out" ]) calc_radius = False points_medial = [] with open('out_axis.off', 'r') as f: header = f.readline() header_s = header.split() p_count = int(header_s[1]) for i in range(0, p_count): line = f.readline() values = line.split() center = PyKDL.Vector(float(values[0]), float(values[1]), float(values[2])) if calc_radius: radius = None for pt in points: dist = (center - pt.pos).Norm() if radius == None or radius > dist: radius = dist points_medial.append([center, radius * 2.0]) else: points_medial.append([center, 0.001]) print len(points_medial) # m_id = 0 # for p in points: # m_id = self.pub_marker.publishSinglePointMarker(PyKDL.Vector(p[0], p[1], p[2]), m_id, r=0, g=1, b=0, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.005, 0.005, 0.005), T=None) # m_id = 0 # for p in points_medial: # m_id = self.pub_marker.publishSinglePointMarker(p[0], m_id, r=1, g=0, b=0, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(p[1],p[1],p[1]), T=None) m_id = 0 m_id = self.pub_marker.publishMultiPointsMarkerWithSize( points_medial, m_id, r=1, g=0, b=0, namespace='default', frame_id='world', m_type=Marker.SPHERE, T=None) # m_id = self.pub_marker.publishMultiPointsMarker(points_medial, m_id, r=1, g=0, b=0, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(0.0005, 0.0005, 0.0005), T=None) rospy.sleep(1) exit(0)
plugins = RaveGetPluginInfo() interfacenames = dict() for type in InterfaceType.values.values(): interfacenames[type] = [] for pluginname, info in plugins: for type, names in info.interfacenames: interfacenames[type] += [(n, pluginname) for n in names] print 'Number of plugins: %d' % len(plugins) for type, names in interfacenames.iteritems(): print '%s: %d' % (str(type), len(names)) names.sort() for interfacename, pluginname in names: print ' %s - %s' % (interfacename, pluginname) RaveDestroy() sys.exit(0) env, viewername = OpenRAVEGlobalArguments.parseAndCreate( options, defaultviewer=True, returnviewer=True) try: # load files after viewer is loaded since they may contain information about where to place the camera for arg in args: env.Load(arg) if options.pythoncmd is not None: eval(compile(options.pythoncmd, '<string>', 'exec')) viewer = None if viewername is not None: if sys.platform.startswith('darwin'): viewer = RaveCreateViewer(env, viewername) env.Add(viewer) else: # create in a separate thread for windows and linux since the signals do not get messed up env.SetViewer(viewername)
parser.add_option('--ipython', '-i',action="store_true",dest='ipython',default=False, help='if true will drop into the ipython interpreter rather than spin') parser.add_option('--mapframe',action="store",type='string',dest='mapframe',default=None, help='The frame of the map used to position the robot. If --mapframe="" is specified, then nothing will be transformed with tf') parser.add_option('--jitter',action="store",type='float',dest='jitter',default=None, help='The jitter to use when moving robot out of collision') parser.add_option('--maxvelmult',action='store',type='float',dest='maxvelmult',default=1.0, help='The maximum velocity multiplier when timing the trajectories (default=%default)') parser.add_option('--wait-for-collisionmap',action='store',type='float',dest='wait_for_collisionmap',default=None, help='Time (seconds) to set. Will wait for the collision map time stamp to reach the same time as the service being called. If 0, will wait indefinitely.') parser.add_option('--request-for-joint_states',action='store',type='string',dest='request_for_joint_states',default='topic', help='whether to get joint states from topic or service. If ="service", will not update robot joint states until receiving service call.') parser.add_option('--use-simulation',action='store',type='string',dest='simulation',default=None, help='if use-simulation is set, we dismiss timestamp of collisionmap.') (options, args) = parser.parse_args() env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=False) RaveLoadPlugin(os.path.join(roslib.packages.get_pkg_dir('orrosplanning'),'lib','liborrosplanning.so')) #RaveLoadPlugin(os.path.join(roslib.packages.get_pkg_dir('openraveros'),'lib','openraveros')) namespace = 'openrave' #env.AddModule(RaveCreateModule(env,'rosserver'),namespace) rospy.loginfo('initializing, please wait for ready signal...') handles = [] # for viewer def UpdateRobotJoints(msg): global options with envlock: with env: for name,pos in izip(msg.name,msg.position): j = robot.GetJoint(name) if j is not None: values[j.GetDOFIndex()] = pos robot.SetDOFValues(values)