Python pybullet.resetBasePositionAndOrientation() Examples

def init_jaco(self, print_joints=False):
        # Enable self collisions to prevent the arm from going through the torso
        if self.task == 'arm_manipulation':
            robot = p.loadURDF(os.path.join(self.directory, 'jaco', 'j2s7s300_gym_arm_manipulation.urdf'), useFixedBase=True, basePosition=[0, 0, 0], flags=p.URDF_USE_SELF_COLLISION, physicsClientId=self.id)
            # Disable collisions between the fingers and the tool
            for i in range(10, 16):
                p.setCollisionFilterPair(robot, robot, i, 9, 0, physicsClientId=self.id)
        else:
            robot = p.loadURDF(os.path.join(self.directory, 'jaco', 'j2s7s300_gym.urdf'), useFixedBase=True, basePosition=[0, 0, 0], flags=p.URDF_USE_SELF_COLLISION, physicsClientId=self.id)
        robot_arm_joint_indices = [1, 2, 3, 4, 5, 6, 7]
        if print_joints:
            self.print_joint_info(robot, show_fixed=True)

        # Initialize and position
        p.resetBasePositionAndOrientation(robot, [-2, -2, 0.975], [0, 0, 0, 1], physicsClientId=self.id)

        # Grab and enforce robot arm joint limits
        lower_limits, upper_limits = self.enforce_joint_limits(robot)

        return robot, lower_limits, upper_limits, robot_arm_joint_indices, robot_arm_joint_indices 

def _reset(self):
        # reset your environment
        
        # reset state
        self.steps = 0
        self.done = False

        # reset morphology
        p.resetBasePositionAndOrientation(self.body, self.initPosition, self.initOrientation) # reset body position and orientation
        
        resetPosition = 0      
        for joint in xrange(self.num_joints):
            if self.random_initial_position:  
                resetPosition = np.random.random() * 2 * np.pi - np.pi
            p.resetJointState(self.body, joint, resetPosition) # reset joint position of joints
        for _ in xrange(100): p.stepSimulation()

        # bootstrap state by running for all repeats
        for i in xrange(self.repeats):
            self.set_state_element_for_repeat(i)
 
        # return this state
        return np.copy(self.state) 

def _addTower(self, pos, blocks, urdf_dir):
        '''
        Helper function that generats a tower containing listed blocks at the
        specific position
        '''
        z = 0.025
        ids = []
        for block in blocks:
            urdf_filename = os.path.join(
                urdf_dir, self.model, self.block_urdf % block)
            obj_id = pb.loadURDF(urdf_filename)
            pb.resetBasePositionAndOrientation(
                obj_id,
                (pos[0], pos[1], z),
                (0, 0, 0, 1))
            self.addObject("block", "%s_block" % block, obj_id)
            z += 0.05
            ids.append(obj_id)
        return ids 

def place(self, pos, rot, joints):
        pb.resetBasePositionAndOrientation(self.handle, pos, rot)
        pb.createConstraint(
            self.handle, -1, -1, -1, pb.JOINT_FIXED, pos, [0, 0, 0], rot)
        for i, q in enumerate(joints):
            pb.resetJointState(self.handle, i, q)

        # gripper
        pb.resetJointState(self.handle, self.left_knuckle, 0)
        pb.resetJointState(self.handle, self.right_knuckle, 0)

        pb.resetJointState(self.handle, self.left_finger, 0)
        pb.resetJointState(self.handle, self.right_finger, 0)

        pb.resetJointState(self.handle, self.left_fingertip, 0)
        pb.resetJointState(self.handle, self.right_fingertip, 0)

        self.arm(joints,)
        self.gripper(0) 

def _addTower(self, pos, blocks, urdf_dir):
        '''
        Helper function that generats a tower containing listed blocks at the
        specific position
        '''
        z = 0.025
        ids = []
        for block in blocks:
            urdf_filename = os.path.join(
                urdf_dir, self.model, self.block_urdf % block)
            obj_id = pb.loadURDF(urdf_filename)
            pb.resetBasePositionAndOrientation(
                obj_id,
                (pos[0], pos[1], z),
                (0, 0, 0, 1))
            self.addObject("block", "%s_block" % block, obj_id)
            z += 0.05
            ids.append(obj_id)
        return ids 

def _addTower(self, pos, blocks, urdf_dir):
        '''
        Helper function that generats a tower containing listed blocks at the
        specific position
        '''
        z = 0.025
        ids = []
        for block in blocks:
            urdf_filename = os.path.join(
                urdf_dir, self.model, self.block_urdf % block)
            obj_id = pb.loadURDF(urdf_filename)
            pb.resetBasePositionAndOrientation(
                obj_id,
                (pos[0], pos[1], z),
                (0, 0, 0, 1))
            self.addObject("block", "%s_block" % block, obj_id)
            z += 0.05
            ids.append(obj_id)
        return ids 

def _flag_reposition(self):
        self.walk_target_x = self.np_random.uniform(low=-self.scene.stadium_halflen,
                                                    high=+self.scene.stadium_halflen)
        self.walk_target_y = self.np_random.uniform(low=-self.scene.stadium_halfwidth,
                                                    high=+self.scene.stadium_halfwidth)

        more_compact = 0.5  # set to 1.0 whole football field
        self.walk_target_x *= more_compact / self.robot.mjcf_scaling
        self.walk_target_y *= more_compact / self.robot.mjcf_scaling

        self.flag = None
        #self.flag = self.scene.cpp_world.debug_sphere(self.walk_target_x, self.walk_target_y, 0.2, 0.2, 0xFF8080)
        self.flag_timeout = 3000 / self.scene.frame_skip
        #print('targetxy', self.flagid, self.walk_target_x, self.walk_target_y, p.getBasePositionAndOrientation(self.flagid))
        #p.resetBasePositionAndOrientation(self.flagid, posObj = [self.walk_target_x, self.walk_target_y, 0.5], ornObj = [0,0,0,0])
        if self.gui:
            if self.lastid:
                p.removeBody(self.lastid)

            self.lastid = p.createMultiBody(baseVisualShapeIndex=self.visualid, baseCollisionShapeIndex=-1, basePosition=[self.walk_target_x, self.walk_target_y, 0.5])

        self.robot.walk_target_x = self.walk_target_x
        self.robot.walk_target_y = self.walk_target_y 

def _addTower(self, pos, blocks, urdf_dir):
        '''
        Helper function that generats a tower containing listed blocks at the
        specific position
        '''
        z = 0.025
        ids = []
        for block in blocks:
            urdf_filename = os.path.join(urdf_dir, self.model, self.block_urdf%block)
            obj_id = pb.loadURDF(urdf_filename)
            pb.resetBasePositionAndOrientation(
                    obj_id,
                    (pos[0], pos[1], z),
                    (0,0,0,1))
            self.addObject("block", "%s_block"%block, obj_id)
            print "%s_block"%block
            z += 0.05
            ids.append(obj_id)
        return ids 

def _setup(self):
        '''
        Create the mug at a random position on the ground, handle facing
        roughly towards the robot. Robot's job is to grab and lift.
        '''

        rospack = rospkg.RosPack()
        path = rospack.get_path('costar_objects')
        sdf_dir = os.path.join(path, self.sdf_dir)
        obj_to_add = os.path.join(sdf_dir, self.model, self.model_file_name)

        identity_orientation = pb.getQuaternionFromEuler([0, 0, 0])
        try:
            obj_id_list = pb.loadSDF(obj_to_add)
            for obj_id in obj_id_list:
                random_position = np.random.rand(
                    3) * self.spawn_pos_delta + self.spawn_pos_min
                pb.resetBasePositionAndOrientation(
                    obj_id, random_position, identity_orientation)
        except Exception, e:
            print e 

def _setup(self):
        '''
        Create the mug at a random position on the ground, handle facing
        roughly towards the robot. Robot's job is to grab and lift.
        '''

        rospack = rospkg.RosPack()
        path = rospack.get_path('costar_simulation')
        urdf_dir = os.path.join(path, self.urdf_dir)
        tray_filename = os.path.join(urdf_dir, self.tray_dir, self.tray_urdf)
        red_filename = os.path.join(urdf_dir, self.model, self.red_urdf)
        blue_filename = os.path.join(urdf_dir, self.model, self.blue_urdf)

        for position in self.tray_poses:
            obj_id = pb.loadURDF(tray_filename)
            pb.resetBasePositionAndOrientation(obj_id, position, (0, 0, 0, 1))

        self._add_balls(self.num_red, red_filename, "red")
        self._add_balls(self.num_blue, blue_filename, "blue") 

def _addTower(self, pos, blocks, urdf_dir):
        '''
        Helper function that generats a tower containing listed blocks at the
        specific position
        '''
        z = 0.025
        ids = []
        for block in blocks:
            urdf_filename = os.path.join(
                urdf_dir, self.model, self.block_urdf % block)
            obj_id = pb.loadURDF(urdf_filename)
            r = self._sampleRotation()
            block_pos = self._samplePos(pos[0], pos[1], z)
            pb.resetBasePositionAndOrientation(
                obj_id,
                block_pos,
                r)
            self.addObject("block", "%s_block" % block, obj_id)
            z += 0.05
            ids.append(obj_id)
        return ids 

def _reset(self):
		# reset state
		self.steps = 0
		self.done = False

		# reset pole on cart in starting poses
		p.resetBasePositionAndOrientation(self.cartpole, (0,0,0.08), (0,0,0,1)) # reset cart position and orientation
		p.resetJointState(self.cartpole, 0, 0) # reset joint position of pole
		for _ in xrange(100): p.stepSimulation()

		# give a fixed force push in a random direction to get things going...
		theta = (np.random.random()*2-1) if self.random_theta else 0.0
		for _ in xrange(self.initial_force_steps):
			p.stepSimulation()
			p.applyExternalForce(self.cartpole, 0, (theta, 0 , 0), (0, 0, 0), p.WORLD_FRAME)
			if self.delay > 0:
				time.sleep(self.delay)

		# bootstrap state by running for all repeats
		for i in xrange(self.repeats):
			self.set_state_element_for_repeat(i)

		# return this state
		return np.copy(self.state) 

def __init__(self, robot_name, print_debug, email_cred_file='', log_file='', control_rate=100, gripper_attached='default'):
        super(WidowXController, self).__init__(robot_name, print_debug, email_cred_file, log_file, control_rate, gripper_attached)
        self._redist_rate = rospy.Rate(50)

        self._arbotix = ArbotiX('/dev/ttyUSB0')
        assert self._arbotix.syncWrite(MAX_TORQUE_L, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring"
        assert self._arbotix.syncWrite(TORQUE_LIMIT, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring"

        self._joint_lock = Lock()
        self._angles, self._velocities = {}, {}
        rospy.Subscriber("/joint_states", JointState, self._joint_callback)
        time.sleep(1)        

        self._joint_pubs = [rospy.Publisher('/{}/command'.format(name), Float64, queue_size=1) for name in JOINT_NAMES[:-1]]
        self._gripper_pub = rospy.Publisher('/gripper_prismatic_joint/command', Float64, queue_size=1)

        p.connect(p.DIRECT)
        widow_x_urdf = '/'.join(__file__.split('/')[:-1]) + '/widowx/widowx.urdf'
        self._armID = p.loadURDF(widow_x_urdf, useFixedBase=True) 
        p.resetBasePositionAndOrientation(self._armID, [0, 0, 0], p.getQuaternionFromEuler([np.pi, np.pi, np.pi]))       
        self._n_errors = 0 

def _reset(self):
		# reset state
		self.steps = 0
		self.done = False

		# reset pole on cart in starting poses
		p.resetBasePositionAndOrientation(self.cart, (0,0,0.08), (0,0,0,1))
		p.resetBasePositionAndOrientation(self.pole, (0,0,0.35), (0,0,0,1))
		for _ in xrange(100): p.stepSimulation()

		# give a fixed force push in a random direction to get things going...
		theta = np.multiply(np.multiply((np.random.random(), np.random.random(), 0),2) - (1,1,0),5) if self.random_theta else (1,0,0)
		
		for _ in xrange(self.initial_force_steps):
			p.stepSimulation()
			p.applyExternalForce(self.pole, -1, theta, (0, 0, 0), p.WORLD_FRAME)
			if self.delay > 0:
				time.sleep(self.delay)

		# bootstrap state by running for all repeats
		for i in xrange(self.repeats):
			self.set_state_element_for_repeat(i)

		# return this state
		return np.copy(self.state) 

def reset_pose(self, position, orientation):
		p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], position, orientation) 

def reset_orientation(self, orientation):
		p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], self.get_position(), orientation) 

def _flag_reposition(self):
        # self.walk_target_x = self.np_random.uniform(low=-self.scene.stadium_halflen,
        #                                            high=+self.scene.stadium_halflen)
        # self.walk_target_y = self.np_random.uniform(low=-self.scene.stadium_halfwidth,
        #                                            high=+self.scene.stadium_halfwidth)
        force_x = self.np_random.uniform(-300, 300)
        force_y = self.np_random.uniform(-300, 300)

        more_compact = 0.5  # set to 1.0 whole football field
        # self.walk_target_x *= more_compact
        # self.walk_target_y *= more_compact

        startx, starty, _ = self.robot.body_xyz

        self.flag = None
        # self.flag = self.scene.cpp_world.debug_sphere(self.walk_target_x, self.walk_target_y, 0.2, 0.2, 0xFF8080)
        self.flag_timeout = 3000 / self.scene.frame_skip
        # print('targetxy', self.flagid, self.walk_target_x, self.walk_target_y, p.getBasePositionAndOrientation(self.flagid))
        # p.resetBasePositionAndOrientation(self.flagid, posObj = [self.walk_target_x, self.walk_target_y, 0.5], ornObj = [0,0,0,0])
        if self.lastid:
            p.removeBody(self.lastid)

        self.lastid = p.createMultiBody(baseMass=1, baseVisualShapeIndex=self.visualid,
                                        baseCollisionShapeIndex=self.colisionid, basePosition=[startx, starty, 0.5])
        p.applyExternalForce(self.lastid, -1, [force_x, force_y, 50], [0, 0, 0], p.LINK_FRAME)

        ball_xyz, _ = p.getBasePositionAndOrientation(self.lastid)

        self.robot.walk_target_x = ball_xyz[0]
        self.robot.walk_target_y = ball_xyz[1] 

def reset_position(self, position):
        p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], position, self.get_orientation()) 

def _flag_reposition(self):
        walk_target_x = self.temp_target_x
        walk_target_y = self.temp_target_y
        walk_target_z = self.robot.get_target_position()[1]

        self.robot.set_target_position([walk_target_x, walk_target_y, walk_target_z])
        self.flag = None
        if not self.gui:
            return

        if self.visual_flagId is None:
            if self.config["display_ui"]:
                self.visual_flagId = -1
            else:
                self.visual_flagId = p.createVisualShape(p.GEOM_MESH, fileName=os.path.join(pybullet_data.getDataPath(), 'cube.obj'), meshScale=[0.5, 0.5, 0.5], rgbaColor=[1, 0, 0, 0.7])
            self.last_flagId = p.createMultiBody(baseVisualShapeIndex=self.visual_flagId, baseCollisionShapeIndex=-1, basePosition=[walk_target_x / self.robot.mjcf_scaling, walk_target_y / self.robot.mjcf_scaling, walk_target_z / self.robot.mjcf_scaling])

            '''
            for i in range(len(ANT_SENSOR_RESULT)):
                walk_target_x, walk_target_y, walk_target_z = ANT_SENSOR_RESULT[i]
                visual_flagId = p.createVisualShape(p.GEOM_MESH, fileName=os.path.join(pybullet_data.getDataPath(), 'cube.obj'), meshScale=[0.2, 0.2, 0.2], rgbaColor=[0.5, 0, 0, 0.7])
                self.last_flagId = p.createMultiBody(baseVisualShapeIndex=visual_flagId, baseCollisionShapeIndex=-1, basePosition=[walk_target_x / self.robot.mjcf_scaling, walk_target_y / self.robot.mjcf_scaling, walk_target_z / self.robot.mjcf_scaling])
            
            for i in range(len(ANT_DEPTH_RESULT)):
                walk_target_x, walk_target_y, walk_target_z = ANT_DEPTH_RESULT[i]
                visual_flagId = p.createVisualShape(p.GEOM_MESH, fileName=os.path.join(pybullet_data.getDataPath(), 'cube.obj'), meshScale=[0.2, 0.2, 0.2], rgbaColor=[0, 0.5, 0, 0.7])
                self.last_flagId = p.createMultiBody(baseVisualShapeIndex=visual_flagId, baseCollisionShapeIndex=-1, basePosition=[walk_target_x / self.robot.mjcf_scaling, walk_target_y / self.robot.mjcf_scaling, walk_target_z / self.robot.mjcf_scaling])
            '''
        else:
            last_flagPos, last_flagOrn = p.getBasePositionAndOrientation(self.last_flagId)
            p.resetBasePositionAndOrientation(self.last_flagId, [walk_target_x  / self.robot.mjcf_scaling, walk_target_y / self.robot.mjcf_scaling, walk_target_z / self.robot.mjcf_scaling], last_flagOrn) 

def _set_fields_of_pose_of(self, pos, orn):
        """Calls native pybullet method for setting real (scaled) robot body pose"""
        p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], np.array(pos) / self.scale, orn) 

def reset_orientation(self, orientation):
        p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], self.get_position(), orientation) 

def _reset(self):
        pass
        # reset your environment
        
#         # reset state
#         self.steps = 0
#         self.done = False
# 
#         # reset pole on cart in starting poses
#         p.resetBasePositionAndOrientation(self.cartpole, (0,0,0.08), (0,0,0,1)) # reset cart position and orientation
#         p.resetJointState(self.cartpole, 0, 0) # reset joint position of pole
#         for _ in xrange(100): p.stepSimulation()
# 
#         # give a fixed force push in a random direction to get things going...
#         theta = (np.random.random()*2-1) if self.random_theta else 0.0
#         for _ in xrange(self.initial_force_steps):
#             p.stepSimulation()
#             p.applyExternalForce(self.cartpole, 0, (theta, 0 , 0), (0, 0, 0), p.WORLD_FRAME)
#             if self.delay > 0:
#                 time.sleep(self.delay)
# 
#         # bootstrap state by running for all repeats
#         for i in xrange(self.repeats):
#             self.set_state_element_for_repeat(i)
# 
#         # return this state
#         return np.copy(self.state) 

def Step(stepIndex):
	for objectId in range(objectNum):
		record = log[stepIndex*objectNum+objectId]
		Id = record[2]
		pos = [record[3],record[4],record[5]]
		orn = [record[6],record[7],record[8],record[9]]
		p.resetBasePositionAndOrientation(Id,pos,orn)
		numJoints = p.getNumJoints(Id)
		for i in range (numJoints):
			jointInfo = p.getJointInfo(Id,i)
			qIndex = jointInfo[3]
			if qIndex > -1:
				p.resetJointState(Id,i,record[qIndex-7+17]) 

def reset_position(self, position):
		p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], position, self.get_orientation()) 

def reset(self):
        '''
        Reset blocks to new random towers. Also resets the world and the
        configuration for all of the new objects, including the robot.
        '''

        # placement = np.random.randint(
        #        0,
        #        len(self.stack_pos),
        #        (len(self.blocks),))
        placement = np.array(range(len(self.stack_pos)))
        np.random.shuffle(placement)
        self.world.done = False
        self.world.ticks = 0

        # loop over all stacks
        # pull out ids now associated with a stack
        for i, pos in enumerate(self.stack_pos):
            blocks = []
            for idx, block in zip(placement, self.block_ids):
                    if idx == i:
                        blocks.append(block)

            # add blocks to tower
            z = 0.025
            for block_id in blocks:
                pb.resetBasePositionAndOrientation(
                    block_id,
                    (pos[0], pos[1], z),
                    (0, 0, 0, 1))
                z += 0.05

        self._setupRobot(self.robot.handle) 

def reset(self):
        '''
        Reset blocks to new random towers
        '''

        #placement = np.random.randint(
        #        0,
        #        len(self.stack_pos),
        #        (len(self.blocks),))
        placement = np.array(range(len(self.stack_pos)))
        #np.random.shuffle(placement)
        self.world.ticks = 0

        # loop over all stacks
        # pull out ids now associated with a stack
        for i, pos in enumerate(self.stack_pos):
            blocks = []
            for idx, block in zip(placement, self.block_ids):
                    if idx == i:
                        blocks.append(block)
                        

            # add blocks to tower
            z = 0.025
            for block_id in blocks:
                pb.resetBasePositionAndOrientation(
                        block_id,
                    (pos[0], pos[1], z),
                    (0,0,0,1))
                z += 0.05
                
        super(DRLBlocksTaskDefinition, self)._setupRobot(self.robot.handle)


        self._setupRobot(self.robot.handle) 

def reset(self):
        '''
        Reset blocks to new random towers. Also resets the world and the
        configuration for all of the new objects, including the robot.
        '''

        # placement = np.random.randint(
        #        0,
        #        len(self.stack_pos),
        #        (len(self.blocks),))
        placement = np.array(range(len(self.stack_pos)))
        np.random.shuffle(placement)
        self.world.done = False
        self.world.ticks = 0

        # loop over all stacks
        # pull out ids now associated with a stack
        for i, pos in enumerate(self.stack_pos):
            blocks = []
            for idx, block in zip(placement, self.block_ids):
                    if idx == i:
                        blocks.append(block)

            # add blocks to tower
            z = 0.025
            for block_id in blocks:
                pb.resetBasePositionAndOrientation(
                    block_id,
                    (pos[0], pos[1], z),
                    (0, 0, 0, 1))
                z += 0.05

        self._setupRobot(self.robot.handle) 

def reset(self):
        for obj_id, position in zip(self.trays, self.tray_poses):
            pb.resetBasePositionAndOrientation(obj_id, position, (0, 0, 0, 1))
        self.robot.place([0, 0, 0], [0, 0, 0, 1], self.joint_positions) 

def _setup(self):
        '''
        Create the mug at a random position on the ground, handle facing
        roughly towards the robot. Robot's job is to grab and lift.
        '''

        rospack = rospkg.RosPack()
        path = rospack.get_path('costar_simulation')
        urdf_dir = os.path.join(path, self.urdf_dir)
        tray_filename = os.path.join(urdf_dir, self.tray_dir, self.tray_urdf)

        for position in self.tray_poses:
            obj_id = pb.loadURDF(tray_filename)
            pb.resetBasePositionAndOrientation(obj_id, position, (0, 0, 0, 1)) 

def reset(self):
        '''
        Reset blocks to new random towers. Also resets the world and the
        configuration for all of the new objects, including the robot.
        '''

        # placement = np.random.randint(
        #        0,
        #        len(self.stack_pos),
        #        (len(self.blocks),))
        placement = np.array(range(len(self.stack_pos)))
        np.random.shuffle(placement)
        self.world.done = False
        self.world.ticks = 0

        # loop over all stacks
        # pull out ids now associated with a stack
        for i, pos in enumerate(self.stack_pos):
            blocks = []
            for idx, block in zip(placement, self.block_ids):
                    if idx == i:
                        blocks.append(block)

            # add blocks to tower
            z = 0.025
            for block_id in blocks:
                pb.resetBasePositionAndOrientation(
                    block_id,
                    (pos[0], pos[1], z),
                    (0, 0, 0, 1))
                z += 0.05

        self._setupRobot(self.robot.handle)