Esempio n. 1
0
 def __init__(self, env_file=None, view=False):
     if env_file is None:
         env_file = rospack.get_path('pr2_utils') + '/robots/my-pr2-beta-sim.robot.xml'
     self.joint_state_msg = None
     self.joint_state_sub = rospy.Subscriber('/joint_states', sm.JointState, self._joint_state_callback)
     
     self.env = rave.Environment()
     self.env.StopSimulation()
     self.env.Load(env_file)
     
     self.added_kinbody_names = list()
     
     self.handles = list()
     self.view = view
     if view:
         self.env.SetViewer('qtcoin')
     
     self.robot = self.env.GetRobots()[0]
     self.larm = self.robot.GetManipulator('leftarm')
     self.rarm = self.robot.GetManipulator('rightarm')
     
     for arm in [self.larm, self.rarm]:
         self.robot.SetActiveManipulator(arm)
     
         ikmodel = rave.databases.inversekinematics.InverseKinematicsModel(self.robot,iktype=rave.IkParameterizationType.Transform6D)
         if not ikmodel.load():
             ikmodel.autogenerate()
Esempio n. 2
0
 def __init__(self, object_io_interface, scene_interface=None, verbose=False, num_hops=2, vis=False):
     self._verbose = verbose
     self._sampler_viewer = vis
     self._orEnv = orpy.Environment() # create openrave environment
     self._orEnv.SetDebugLevel(orpy.DebugLevel.Fatal)
     self._orEnv.GetCollisionChecker().SetCollisionOptions(orpy.CollisionOptions.Contacts)
     if vis:
         self._orEnv.SetViewer('qtcoin') # attach viewer (optional)
         self._or_handles = []
     else:
         self._or_handles = None
     self._scene_or_env = None
     self._hand_loaded = False
     self._scene_interface = scene_interface
     self._obj_loaded = False
     self._max_iters = 40
     self._reachability_weight = 1.0
     self._mu = 2.0
     self._min_stability = 0.0
     self._b_force_new_hfts = False
     self._object_kd_tree = None
     self._object_points = None
     # self._hops = num_hops
     # TODO remove this aga
     self._hops = 2
     self._robot = None
     self._obj = None
     self._obj_com = None
     self._data_labeled = None
     self._hand_manifold = None
     self._num_contacts = None
     self._contact_combinations = []
     self._num_levels = 0
     self._branching_factors = []
     self._object_io_interface = object_io_interface
def main():
    """Loads an environment and generates random positions until the robot can
    reach an oject from a collision-free pose.
    """
    
    env = openravepy.Environment()    
    env.Load('data/pr2test1.env.xml')
    env.SetViewer('qtcoin')
    robot=env.GetRobots()[0]
    manip = robot.SetActiveManipulator('rightarm')
    
    pose = generate_random_pos(robot) 
    robot.SetTransform(pose)        

    obj = env.GetKinBody('mug1')
    #env.GetKinBody('obstacle').SetVisible(False)
    
    try:
        pose, sol, torso= get_collision_free_grasping_pose(robot,
                                                     obj,
                                                     100)
        robot.SetTransform(pose)
        robot.SetDOFValues(sol, manip.GetArmIndices())        
        openravepy.raveLogInfo("Done!!")        
    except ValueError, e:
        openravepy.raveLogError("Error while trying to find a pose: %s" % e)
        return
Esempio n. 4
0
    def __init__(self, view=False):
        env_file = os.path.join(rll_quadrotor_folder,
                                'simulation/models/trajopt_quadrotor.xml') # TODO

        self.env = rave.Environment()
        physics = rave.RaveCreatePhysicsEngine(self.env,'bullet')
        self.env.SetPhysicsEngine(physics)
        self.env.StopSimulation()
        self.env.Load(env_file)

        self.kinbodies = set([b for b in self.env.GetBodies() if not b.IsRobot()])

        self.handles = list()
        self.view = view
        self.viewer = None
        if view:
            self.env.SetViewer('qtcoin')
            self.viewer = self.env.GetViewer()
            time.sleep(0.1)

        assert(len(self.env.GetRobots()) == 1)
        self.robot = self.env.GetRobots()[0]

        self.cc = trajoptpy.GetCollisionChecker(self.env)

        self.aabb_cache = dict()
        self.is_watertight_cache = dict()
Esempio n. 5
0
def pr2_flashlidar():
    env = rave.Environment()
    #env.Load('envs/testpr2sensors.env.xml')
    env.Load('envs/pr2-table.env.xml')

    env.SetViewer('qtcoin')
    time.sleep(1)

    start_time = time.time()
    sensors = [
        s for s in env.GetSensors() if s.GetName().find("flashlidar") != -1
    ]
    lidar = sensors[0]

    lidar.Configure(Sensor.ConfigureCommand.PowerOn)
    #lidar.Configure(Sensor.ConfigureCommand.RenderDataOn)

    while True:
        start_time = time.time()
        olddata = lidar.GetSensorData(Sensor.Type.Laser)
        while True:
            data = lidar.GetSensorData(Sensor.Type.Laser)
            if data.stamp != olddata.stamp:
                break
            time.sleep(0.1)
        print('Elapsed: {0}'.format(time.time() - start_time))
        break

    lidar.Configure(Sensor.ConfigureCommand.PowerOff)
    #lidar.Configure(Sensor.ConfigureCommand.RenderDataOff)

    IPython.embed()
Esempio n. 6
0
def main():
    global e
    global m
    global robot
    global knownstates

    e = r.Environment()
    m = r.RaveCreateModule(e, 'orcdchomp')

    e.SetViewer('qtcoin')
    #e.Load( 'lab1.env.xml' );

    e.Load('herb2_padded_nosensors.robot.xml')
    robot = e.GetRobot('Herb2')

    knownstates = readKnownStates("data/knownstates.data")
    printKnownStates(knownstates)

    return

    #robot.SetActiveManipulator( 0 )

    filename = "randomstates2.data"
    #filename = "randomManipulatorStates_0.data"

    userGetRandomStates(filename)

    return
Esempio n. 7
0
def eih():
    env = rave.Environment()
    env.Load('data/testwamcamera.env.xml')

    env.SetViewer('qtcoin')

    ienablesensor = 0
    while True:
        sensors = env.GetSensors()
        for i, sensor in enumerate(sensors):
            if i == ienablesensor:
                sensor.Configure(Sensor.ConfigureCommand.PowerOn)
                sensor.Configure(Sensor.ConfigureCommand.RenderDataOn)
            else:
                sensor.Configure(Sensor.ConfigureCommand.PowerOff)
                sensor.Configure(Sensor.ConfigureCommand.RenderDataOff)
        print 'showing sensor %s, try moving obstacles' % (
            sensors[ienablesensor].GetName())
        if sensors[ienablesensor].Supports(Sensor.Type.Laser):
            # if laser, wait for the sensor data to be updated and then print it
            olddata = sensors[ienablesensor].GetSensorData(Sensor.Type.Laser)
            while True:
                data = sensors[ienablesensor].GetSensorData(Sensor.Type.Laser)
                if data.stamp != olddata.stamp:
                    break
                time.sleep(0.1)
            print 'sensor data: ', data.ranges
        time.sleep(5)
        ienablesensor = (ienablesensor + 1) % len(sensors)

    IPython.embed()
Esempio n. 8
0
	def __init__(self, databaseutils=None, 
			   robotName="care-o-bot3.zae", 
			   robotManipulator="arm",
			   env=None,
			   robot=None,
			   pregrasp_offset=0.2):

		self.robotName = roslib.packages.get_pkg_dir("srs_grasping")+"/robots/"+robotName;
		self.manipulator = robotManipulator;
		self.env = env;
		self.robot = robot;
		self.pregrasp_offset = pregrasp_offset;
		self.databaseutils = databaseutils


		if self.databaseutils is None:
			self.databaseutils = grasping_functions.databaseutils();

		self.graspingutils = self.databaseutils.get_graspingutils();

		try:
			if self.env is None:
				self.env = openravepy.Environment();
			if self.robot is None:
				self.robot = self.env.ReadRobotXMLFile(self.robotName);
			
			self.robot.SetActiveManipulator(self.manipulator);
			self.env.AddRobot(self.robot);
		except:
			rospy.logerr("The robot file %s does not exists.", self.robotName);
			return GraspingErrorCodes.CORRUPTED_ROBOT_MESH_FILE;
Esempio n. 9
0
File: PR2.py Progetto: zzz622848/lfd
    def __init__(self):

        # set up openrave
        self.env = rave.Environment()
        self.env.StopSimulation()
        self.env.Load(
            "robots/pr2-beta-static.zae")  # todo: use up-to-date urdf
        self.robot = self.env.GetRobots()[0]

        self.joint_listener = TopicListener("/joint_states", sm.JointState)

        # rave to ros conversions
        joint_msg = self.get_last_joint_message()
        ros_names = joint_msg.name
        inds_ros2rave = np.array(
            [self.robot.GetJointIndex(name) for name in ros_names])
        self.good_ros_inds = np.flatnonzero(
            inds_ros2rave != -1)  # ros joints inds with matching rave joint
        self.rave_inds = inds_ros2rave[
            self.
            good_ros_inds]  # openrave indices corresponding to those joints
        self.update_rave()

        self.larm = Arm(self, "l")
        self.rarm = Arm(self, "r")
        self.lgrip = Gripper(self, "l")
        self.rgrip = Gripper(self, "r")
        self.head = Head(self)
        self.torso = Torso(self)
        self.base = Base(self)

        rospy.on_shutdown(self.stop_all)
Esempio n. 10
0
def test_camera():
    env = rave.Environment()
    env.Load('../envs/pr2-empty.env.xml')
    env.SetViewer('qtcoin')
    #env.GetViewer().SendCommand('SetFiguresInCamera 1') # also shows the figures in the image
    time.sleep(1)
    robot = env.GetRobots()[0]

    sensor = robot.GetAttachedSensor('head_cam').GetSensor()

    cam = Camera(robot, sensor)

    is_hits, hits = cam.get_hits(subsampled=True)
    zbuffer = cam.get_zbuffer()

    height, width, _ = hits.shape
    global handles
    for i in xrange(height):
        for j in xrange(width):
            #print zpoints[i,j,:]
            if is_hits[i, j]:
                if cam.is_in_fov(hits[i, j, :], zbuffer):
                    handles += [utils.plot_point(env, hits[i, j, :], size=.01)]

    IPython.embed()
Esempio n. 11
0
 def __init__(self):
     self.env = openravepy.Environment()
     self.env.SetViewer('qtcoin')
     self.env.GetViewer().SetName('Tutorial Viewer')
     self.env.Load('models/%s.env.xml' % PACKAGE_NAME)
     # time.sleep(3) # wait for viewer to initialize. May be helpful to uncomment
     self.robot = self.env.GetRobots()[0]
Esempio n. 12
0
def load(source, content_type='application/zip'):

    # determine content type
    suffix = ''
    if content_type == 'application/xml':
        suffix = '.xml'
    if content_type == 'application/zip':
        suffix = '.zae'

    # write source into a file, then load it in openrave
    env = openravepy.Environment()
    with tempfile.NamedTemporaryFile(suffix=suffix) as f:
        f.write(source)
        f.flush()

        if not env.Load(f.name):
            raise ValueError('Failed to parse the file.')

    # extract information from the robot
    data = {}
    with env:
        robot = env.GetRobots()[0]
        data = {
            'name': robot.GetName(),
            'description': robot.GetDescription(),
            'dof': robot.GetDOF(),
            'sensors': [sensor.GetName() for sensor in robot.GetSensors()],
        }

    return data
Esempio n. 13
0
    def setUp(self):
        import openravepy

        if not self.is_setup:
            openravepy.RaveInitialize(True)
            openravepy.misc.InitOpenRAVELogging()
            openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Fatal)
            self.is_setup = True

        self.env = openravepy.Environment()
        with self.env:
            self.env.Load('data/wamtest2.env.xml')
            self.robot = self.env.GetRobot('BarrettWAM')
            self.manipulator = self.robot.GetManipulator('arm')

            self.env.Remove(self.env.GetKinBody('floor'))

            self.robot.SetActiveManipulator(self.manipulator)
            self.robot.SetActiveDOFs(self.active_dof_indices)

        self.planner = self.planner_factory()

        for base_cls in self.__class__.__bases__:
            if base_cls != BasePlannerTest and hasattr(base_cls, 'setUp'):
                base_cls.setUp(self)
def env():
    """Simple openrave environment."""
    rave_env = orpy.Environment()
    rave_env.Load('data/lab1.env.xml')
    rave_env.GetRobots()[0].SetActiveDOFs(range(7))
    yield rave_env
    rave_env.Destroy()
Esempio n. 15
0
def test_cd():
    global handles
    env = rave.Environment()
    env.Load('../envs/pr2-test.env.xml')
    env.SetViewer('qtcoin')

    mug = env.GetKinBody('mug')
    mug_pos = tfx.pose(mug.GetTransform()).position.array
    mugcd = rave.databases.convexdecomposition.ConvexDecompositionModel(mug)
    if not mugcd.load():
        mugcd.autogenerate()

    mugcd_trimesh = mugcd.GenerateTrimeshFromHulls(mugcd.linkgeometry[0][0][1])

    new_mug = rave.RaveCreateKinBody(env, '')
    new_mug.SetName('new_mug')
    new_mug.InitFromTrimesh(mugcd_trimesh)
    new_mug.SetTransform(mug.GetTransform())
    #env.Add(new_mug, True)
    env.Remove(mug)

    I, V = mugcd_trimesh.indices, mugcd_trimesh.vertices
    for indices in I:
        v0 = mug_pos + V[indices[0], :]
        v1 = mug_pos + V[indices[1], :]
        v2 = mug_pos + V[indices[2], :]

        handles += utils.plot_segment(env, v0, v1)
        handles += utils.plot_segment(env, v1, v2)
        handles += utils.plot_segment(env, v2, v0)

    IPython.embed()
Esempio n. 16
0
def pr2_sensors():
    env = rave.Environment()
    #env.Load('robots/pr2-beta-sim.robot.xml')
    env.Load('envs/testpr2sensors.env.xml')
    r = env.GetRobots()[0]

    env.SetViewer('qtcoin')

    ienablesensor = 0
    while True:
        start_time = time.time()
        sensors = env.GetSensors()
        for i, sensor in enumerate(sensors):
            if i == ienablesensor:
                sensor.Configure(Sensor.ConfigureCommand.PowerOn)
                sensor.Configure(Sensor.ConfigureCommand.RenderDataOn)
            else:
                sensor.Configure(Sensor.ConfigureCommand.PowerOff)
                sensor.Configure(Sensor.ConfigureCommand.RenderDataOff)
        print 'showing sensor %s, try moving obstacles' % (
            sensors[ienablesensor].GetName())
        if sensors[ienablesensor].Supports(Sensor.Type.Laser):
            # if laser, wait for the sensor data to be updated and then print it
            olddata = sensors[ienablesensor].GetSensorData(Sensor.Type.Laser)
            while True:
                data = sensors[ienablesensor].GetSensorData(Sensor.Type.Laser)
                if data.stamp != olddata.stamp:
                    break
                time.sleep(0.1)
            print 'sensor data: ', data.ranges
        #time.sleep(5)
        ienablesensor = (ienablesensor + 1) % len(sensors)
        print('Elapsed: {0}'.format(time.time() - start_time))

    IPython.embed()
Esempio n. 17
0
    def setUp(self):
        self.env = openravepy.Environment()
        self.env.Load('wamtest1.env.xml')
        self.robot = self.env.GetRobot('BarrettWAM')
        self.manipulator = self.robot.GetManipulator('arm')

        self.robot.Enable(True)

        # Set all 7 DOF of the WAM arm to active
        with self.env:
            self.robot.SetActiveDOFs(self.manipulator.GetArmIndices())
            self.robot.SetActiveManipulator(self.manipulator)
            self.active_dof_indices = self.robot.GetActiveDOFIndices()

        # Get the resolution (in radians) for the 7 joints
        # [0.0043, 0.0087, 0.0087, 0.0174, 0.0193, 0.0282, 0.0282]
        self.dof_resolutions = \
               self.robot.GetDOFResolutions()[self.active_dof_indices]


        #add the box object we will grab
        self.to_grab_body = openravepy.RaveCreateKinBody(self.env, '')
        self.to_grab_body.SetName('box')
        self.to_grab_body.InitFromBoxes(numpy.array([[0., 0., 0., 0.1, 0.1, 0.1]]), False)
        self.env.Add(self.to_grab_body)

        self.to_grab_body.Enable(True)
        T = numpy.eye(4)
        T[2,3] = 20. #move far to not be in collision
        self.to_grab_body.SetTransform(T)
Esempio n. 18
0
    def __init__(self):
        self.env = rave.Environment()
        self.env.SetViewer('qtcoin')

        rospy.loginfo('Before loading model')
        ravenFile = os.path.join(roslib.packages.get_pkg_subdir('RavenDebridement','models'),'myRaven.xml')
        
        #ravenFile = '/home/gkahn/ros_workspace/RavenDebridement/models/myRaven.xml'
        self.env.Load(ravenFile)
        rospy.loginfo('After loading model')
        
        self.robot = self.env.GetRobots()[0]
        
        # setup variables for kinect constraint generation
        self.num_cd_components = 50
        self.geom_type = 'cd'
        self.kinect_topic = '/camera/depth/points'
        self.kinect_depth_frame = '/camera_depth_optical_frame'
        self.robot_frame = '/world'
        self.T_kinect_to_robot = tfx.lookupTransform(self.robot_frame, self.kinect_depth_frame, wait=20)
        
        self.cd_body_names = []
        self.cloud_id = 0
        
        rospy.Subscriber(self.kinect_topic, PointCloud2, self.kinect_callback)
Esempio n. 19
0
 def setUpClass(cls):
   np.set_printoptions(precision=6, suppress=True)
   env = orpy.Environment()
   makita = env.ReadKinBodyXMLFile('objects/makita.kinbody.xml')
   env.AddKinBody(makita)
   cls.env = env
   print('') # dummy line
Esempio n. 20
0
    def __init__(self, showViewer=True):
        '''Initializes openrave environment, etc.'''

        # Parameters

        self.projectDir = "/home/mgualti/mgualti/PickAndPlace/simulation"

        # Create openrave environment

        self.env = openravepy.Environment()
        if showViewer: self.env.SetViewer('qtcoin')
        self.showViewer = showViewer
        self.env.Load(self.projectDir + "/openrave/environment_1.xml")
        self.robot = self.env.GetRobots()[0]
        self.robot.SetDOFValues(array([0.034]))

        # don't want to be affected by gravity, since it is floating
        for link in self.robot.GetLinks():
            link.SetStatic(True)

        #self.physicsEngine = openravepy.RaveCreatePhysicsEngine(self.env, "ode")
        #self.env.SetPhysicsEngine(self.physicsEngine)
        #self.env.GetPhysicsEngine().SetGravity([0,0,-9.8])
        #self.env.GetPhysicsEngine().SetGravity([0,0,0])
        self.env.StopSimulation()

        tableObj = self.env.GetKinBody("table")
        self.tablePosition = tableObj.GetTransform()[0:3, 3]
        self.tableExtents = tableObj.ComputeAABB().extents()
 def __init__(self, args):
   # Initialize values
   self.busy = False
   self.topic_name = args.topic
   self.file_name = args.file_name
   self.octomap_resolution = str(args.resolution)
   self.octomap_range = str(args.range)
   self.octomap_frame = args.frame
   # env_name = '' # TODO:
   # robot_name = '' # TODO:
   # obstacle = '' # TODO:
   self.durations = []
   # Initialize OpenRAVE environment
   self.env = orpy.Environment()
   self.env.SetViewer('qtcoin')
   orpy.RaveSetDebugLevel(orpy.DebugLevel.Error)
   # self.env.Load(env_name)
   # Initialize ROS subscribers and publisher
   self.sub_octomap = rospy.Subscriber('/occupied_cells_vis_array',
                                       MarkerArray,
                                       callback=self.process_octomap,
                                       queue_size=1)
   # Initialize octomap plugin
   orpy.RaveLoadPlugin("or_octomap")  # Load octomap plugin
   self.sensor_server = orpy.RaveCreateSensorSystem(self.env, "or_octomap")
   self.sensor_server.SendCommand("Enable") # Enable octomap thread
   self.sensor_server.SendCommand("ResetTopic " +
                       self.topic_name) # Reset the registered pcl topic
   self.sensor_server.SendCommand("ResetResolution " +
                       self.octomap_resolution) # Reset the octomap resolution
   self.sensor_server.SendCommand("ResetRange " +
                       self.octomap_range) # Reset the octomap max range
   self.sensor_server.SendCommand("ResetFrameID " +
                       self.octomap_frame) # Reset the octomap frame
   self.time_start = time.time()
Esempio n. 22
0
    def openrave_init(self):
        self.env = openravepy.Environment()
        self.env.SetViewer('qtcoin')
        self.env.GetViewer().SetName('HW1 Viewer')
        self.env.Load(curr_path + '/models/%s.env.xml' % PACKAGE_NAME)

        # time.sleep(3) # wait for viewer to initialize. May be helpful to
        # uncomment
        self.robot_left = self.env.GetRobots()[0]
        self.robot_right = self.env.GetRobots()[1]

        right_relaxed = [5.65, -1.76, -0.26, 1.96, -1.15, 0.87, -1.43]
        left_relaxed = [0.64, -1.76, 0.26, 1.96, 1.16, 0.87, 1.43]

        self.right_manip = self.robot_right.GetManipulator('rightarm')
        # self.right_manip = self.robot_right.GetManipulator('right_wam')
        # self.robot.SetActiveDOFs(right_manip.GetArmIndices())
        # self.robot.SetActiveDOFValues(right_relaxed)

        self.left_manip = self.robot_left.GetManipulator('leftarm')
        # self.robot.SetActiveDOFs(left_manip.GetArmIndices())
        # self.robot.SetActiveDOFValues(left_relaxed)

        self.robot_right.SetActiveManipulator('rightarm')
        self.robot_left.SetActiveManipulator('leftarm')
Esempio n. 23
0
def main():
    global m
    global e

    e = r.Environment()
    m = r.RaveCreateModule(e, 'orchomp')
    e.SetViewer('qtcoin')

    e.Load("intelkitchen_robotized_herb2_nosensors.env.xml")
    robot = e.GetRobot("Herb2")

    # set the manipulator to leftarm
    #ikmodel = databases.inversekinematics.InverseKinematicsModel(
    #            robot,iktype=IkParameterization.Type.Transform6D)
    #if not ikmodel.load():
    #    ikmodel.autogenerate()

    Tz = r.matrixFromAxisAngle([0, 0, numpy.pi / 2])
    Tz[0, 3] = 0.4
    Tz[1, 3] = 1.6
    Tz[2, 3] = -0.01

    print Tz
    with e:
        for body in e.GetBodies():
            body.SetTransform(numpy.dot(Tz, body.GetTransform()))

    readFileCommands()
def main():
    global get_motion_plan, env, robot, markerpub
    if rospy.get_name() == "/unnamed":
        rospy.init_node("move_group_battery")
    env = rave.Environment()
    env.Load("robot/pr2-beta-static.zae")
    loadsuccess = env.Load(envfile)
    assert loadsuccess
    markerpub = rospy.Publisher('battery_targets', Marker)
    get_motion_plan = rospy.ServiceProxy('plan_kinematic_path', GetMotionPlan)
    print "waiting for plan_kinematic_path"
    get_motion_plan.wait_for_service()
    print "ok"

    robot = env.GetRobots()[0]
    update_rave_from_ros(robot, ROS_DEFAULT_JOINT_VALS, ROS_JOINT_NAMES)

    xs, ys, zs = np.mgrid[.35:.65:.05, 0:.5:.05, .8:.9:.1]

    results = []
    for (x, y, z) in zip(xs.flat, ys.flat, zs.flat):
        result = test_plan_to_pose([x, y, z], [0, 0, 0, 1],
                                   "left",
                                   robot,
                                   planner_id=args.planner_id)
        # publish_marker(x, y, z)
        if result is not None: results.append(result)
    process_results(results)
Esempio n. 25
0
def initialize(model_filename='jaco', envXML=None):
	'''
	Load and configure the JACO robot. If envXML is not None, loads environment.
	Returns robot and environment.
	'''
	env = openravepy.Environment()
	if envXML is not None:
		env.LoadURI(envXML)
	env.SetViewer('qtcoin')

	# Assumes the robot files are located in the data folder of the
	# kinova_description package in the catkin workspace.
	urdf_uri = 'package://iact_control/src/data/'+model_filename+'.urdf'
	srdf_uri = 'package://iact_control/src/data/'+model_filename+'.srdf'
	or_urdf = openravepy.RaveCreateModule(env, 'urdf')
	robot_name = or_urdf.SendCommand('load {:s} {:s}'.format(urdf_uri, srdf_uri))
	robot = env.GetRobot(robot_name)
	bind_subclass(robot, ArchieRobot)

	robot.SetActiveDOFs(np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9]))
	robot.SetDOFValues(robot_starting_dofs)

	viewer = env.GetViewer()
	viewer.SetSize(500,500)
	cam_params = np.array([[-0.99885711, -0.01248719, -0.0461361 , -0.18887213],
		   [ 0.02495645,  0.68697757, -0.72624996,  2.04733515],
		   [ 0.04076329, -0.72657133, -0.68588079,  1.67818344],
		   [ 0.        ,  0.        ,  0.        ,  1.        ]])
	viewer.SetCamera(cam_params)
	#viewer.SetBkgndColor([0.8,0.8,0.8])

	return env, robot
Esempio n. 26
0
    def wam7_test(self):
        env = openravepy.Environment()
        try:
            robot = env.ReadRobotXMLFile('robots/barrettwam.robot.xml')
            env.Add(robot)
            robot.SetActiveDOFs(robot.GetManipulator('arm').GetArmIndices())
            robot.SetActiveDOFValues(q_start)
            planner = openravepy.RaveCreatePlanner(env, 'LEMUR')
            self.assertIsNotNone(planner)

            params = openravepy.Planner.PlannerParameters()
            params.SetRobotActiveJoints(robot)
            params.SetGoalConfig(q_goal)
            params.SetExtraParameters(
                '<roadmap_type>Halton</roadmap_type>' +
                '<roadmap_param>num=1000</roadmap_param>' +
                '<roadmap_param>radius=2.0</roadmap_param>')
            planner.InitPlan(robot, params)

            traj = openravepy.RaveCreateTrajectory(env, '')
            result = planner.PlanPath(traj)
            self.assertEqual(result, openravepy.PlannerStatus.HasSolution)

            self.assertEqual(6, traj.GetNumWaypoints())
            for iwp in range(6):
                wp = traj.GetWaypoint(iwp)
                self.assertEqual(7, len(wp))
                for a, b in zip(wp, traj_expected[iwp]):
                    self.assertAlmostEqual(a, b, delta=1.0e-5)

        finally:
            env.Destroy()
Esempio n. 27
0
def main(env=None, test=False):
    "Main function."
    parser = argparse.ArgumentParser(description="A simple example in which trajectories, which are planned using"
                                                 "OpenRAVE is retimed using toppra. The trajectories are found using"
                                                 "birrt, the default planner. Goals are generated randomly and "
                                                 "uniformly.")
    parser.add_argument('-e', '--env', help='OpenRAVE Environment file', default="data/lab1.env.xml")
    parser.add_argument('-v', '--verbose', help='Show DEBUG log and plot trajectories', action="store_true")
    parser.add_argument('-N', '--Ngrid', help='Number of discretization step', default=100, type=int)
    args = vars(parser.parse_args())

    if args['verbose']:
        toppra.setup_logging('DEBUG')
    else:
        toppra.setup_logging('INFO')

    if env is None:
        env = orpy.Environment()
    env.SetDebugLevel(0)
    env.Load(args['env'])
    env.SetViewer('qtosg')
    robot = env.GetRobots()[0]
    robot.SetDOFAccelerationLimits(np.ones(11) * 3)
    manipulator = robot.GetManipulators()[0]
    robot.SetActiveManipulator(manipulator)
    robot.SetActiveDOFs(manipulator.GetArmIndices())
    controller = robot.GetController()
    basemanip = orpy.interfaces.BaseManipulation(robot)
    dof = robot.GetActiveDOF()

    pc_torque = toppra.create_rave_torque_path_constraint(
        robot, discretization_scheme=toppra.constraint.DiscretizationType.Interpolation)

    it = 0
    while True or (test and it > 5):
        lower, upper = robot.GetActiveDOFLimits()
        qrand = np.random.rand(dof) * (upper - lower) + lower
        with robot:
            robot.SetActiveDOFValues(qrand)
            incollision = env.CheckCollision(robot) or robot.CheckSelfCollision()
            if incollision:
                continue
        traj_original = basemanip.MoveActiveJoints(qrand, execute=False, outputtrajobj=True)
        if traj_original is None:
            continue
        traj_retimed, trajra = toppra.retime_active_joints_kinematics(
            traj_original, robot, output_interpolator=True, N=args['Ngrid'],
            additional_constraints=[pc_torque])

        print(("Original duration: {:.3f}. Retimed duration: {:3f}.".format(
            traj_original.GetDuration(), traj_retimed.GetDuration())))

        if args['verbose']:
            plot_trajectories(traj_original, traj_retimed, robot)
        time.sleep(1)

        controller.SetPath(traj_retimed)
        robot.WaitForController(0)
        it += 1
Esempio n. 28
0
 def setUp(self):
     self.env = openravepy.Environment()
     self.fuze = prpy.rave.add_object(self.env, 'fuze_bottle',
                                      'objects/fuze_bottle.kinbody.xml')
     self.fuze_pose = numpy.eye(4)
     self.fuze_pose[0:3, 3] = [0.8, -0.3, 0.4]
     self.fuze.SetTransform(self.fuze_pose)
     self.fuze.Enable(True)
Esempio n. 29
0
    def __init__(self):
        # set up openrave
        self.env = rave.Environment()
        self.env.Load("robots/pr2-beta-static.zae")
        self.robot = self.env.GetRobots()[0]

        self.larm = Arm(self, "l")
        self.rarm = Arm(self, "r")
Esempio n. 30
0
    def setUp(self):
        self.env = openravepy.Environment()
        urdf_module = openravepy.RaveCreateModule(self.env, 'urdf')

        with self.env:
            arg = 'load {:s}'.format(urdf_path)
            bh_name = urdf_module.SendCommand(arg)
            self.bh_body = self.env.GetKinBody(bh_name)