Python pybullet.getQuaternionFromEuler方法代碼示例

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import getQuaternionFromEuler [as 別名]
def _reset(self):
    #print("KukaGymEnv _reset")
    self.terminated = 0
    p.resetSimulation()
    p.setPhysicsEngineParameter(numSolverIterations=150)
    p.setTimeStep(self._timeStep)
    p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1])

    p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)

    xpos = 0.55 +0.12*random.random()
    ypos = 0 +0.2*random.random()
    ang = 3.14*0.5+3.1415925438*random.random()
    orn = p.getQuaternionFromEuler([0,0,ang])
    self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.15,orn[0],orn[1],orn[2],orn[3])

    p.setGravity(0,0,-10)
    self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
    self._envStepCounter = 0
    p.stepSimulation()
    self._observation = self.getExtendedObservation()
    return np.array(self._observation) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import getQuaternionFromEuler [as 別名]
def _reset(self):
    self.terminated = 0
    p.resetSimulation()
    p.setPhysicsEngineParameter(numSolverIterations=150)
    p.setTimeStep(self._timeStep)
    p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1])

    p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)

    xpos = 0.5 +0.2*random.random()
    ypos = 0 +0.25*random.random()
    ang = 3.1415925438*random.random()
    orn = p.getQuaternionFromEuler([0,0,ang])
    self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3])

    p.setGravity(0,0,-10)
    self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
    self._envStepCounter = 0
    p.stepSimulation()
    self._observation = self.getExtendedObservation()
    return np.array(self._observation) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import getQuaternionFromEuler [as 別名]
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 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import getQuaternionFromEuler [as 別名]
def reset(self, height=0.5, orientation='straight'):
        """Resets the robot for experiment (joints, robot position, ball position, simulator time)
        
        Keyword Arguments:
            height {float} -- height of the reset (m) (default: {0.55})
            orientation {str} -- orientation (straight, front or back) of the robot (default: {'straight'})
        """
        self.lines = []
        self.t = 0
        self.start = time.time()

        # Resets the robot position
        orn = [0, 0, 0]
        if orientation == 'front':
            orn = [0, math.pi/2, 0]
        elif orientation == 'back':
            orn = [0, -math.pi/2, 0]
        self.resetPose([0, 0, height], p.getQuaternionFromEuler(orn))

        # Reset the joints to 0
        for entry in self.joints.values():
            p.resetJointState(self.robot, entry, 0) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import getQuaternionFromEuler [as 別名]
def ik_random_restarts(self, body, target_joint, target_pos, target_orient, world_creation, robot_arm_joint_indices, robot_lower_limits, robot_upper_limits, ik_indices=range(29, 29+7), max_iterations=1000, max_ik_random_restarts=50, random_restart_threshold=0.01, half_range=False, step_sim=False, check_env_collisions=False):
        orient_orig = target_orient
        best_ik_joints = None
        best_ik_distance = 0
        for r in range(max_ik_random_restarts):
            target_joint_positions = self.ik(body, target_joint, target_pos, target_orient, ik_indices=ik_indices, max_iterations=max_iterations, half_range=half_range)
            world_creation.setup_robot_joints(body, robot_arm_joint_indices, robot_lower_limits, robot_upper_limits, randomize_joint_positions=False, default_positions=np.array(target_joint_positions), tool=None)
            if step_sim:
                for _ in range(5):
                    p.stepSimulation(physicsClientId=self.id)
                if len(p.getContactPoints(bodyA=body, bodyB=body, physicsClientId=self.id)) > 0 and orient_orig is not None:
                    # The robot's arm is in contact with itself. Continually randomize end effector orientation until a solution is found
                    target_orient = p.getQuaternionFromEuler(p.getEulerFromQuaternion(orient_orig, physicsClientId=self.id) + np.deg2rad(self.np_random.uniform(-45, 45, size=3)), physicsClientId=self.id)
            if check_env_collisions:
                for _ in range(25):
                    p.stepSimulation(physicsClientId=self.id)
            gripper_pos, gripper_orient = p.getLinkState(body, target_joint, computeForwardKinematics=True, physicsClientId=self.id)[:2]
            if np.linalg.norm(target_pos - np.array(gripper_pos)) < random_restart_threshold and (target_orient is None or np.linalg.norm(target_orient - np.array(gripper_orient)) < random_restart_threshold or np.isclose(np.linalg.norm(target_orient - np.array(gripper_orient)), 2, atol=random_restart_threshold)):
                return True, np.array(target_joint_positions)
            if best_ik_joints is None or np.linalg.norm(target_pos - np.array(gripper_pos)) < best_ik_distance:
                best_ik_joints = target_joint_positions
                best_ik_distance = np.linalg.norm(target_pos - np.array(gripper_pos))
        world_creation.setup_robot_joints(body, robot_arm_joint_indices, robot_lower_limits, robot_upper_limits, randomize_joint_positions=False, default_positions=np.array(best_ik_joints), tool=None)
        return False, np.array(best_ik_joints) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import getQuaternionFromEuler [as 別名]
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 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import getQuaternionFromEuler [as 別名]
def _reset(self):
        # reset is called once at initialization of simulation
        self.vt = 0
        self.vd = 0
        self.maxV = 24.6 # 235RPM = 24,609142453 rad/sec
        self._envStepCounter = 0

        p.resetSimulation()
        p.setGravity(0,0,-10) # m/s^2
        p.setTimeStep(0.01) # sec
        planeId = p.loadURDF("plane.urdf")
        cubeStartPos = [0,0,0.001]
        cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])

        path = os.path.abspath(os.path.dirname(__file__))
        self.botId = p.loadURDF(os.path.join(path, "balancebot_simple.xml"),
                           cubeStartPos,
                           cubeStartOrientation)

        # you *have* to compute and return the observation from reset()
        self._observation = self._compute_observation()
        return np.array(self._observation) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import getQuaternionFromEuler [as 別名]
def _randomly_place_objects(self, urdfList):
    """Randomly places the objects in the bin.

    Args:
      urdfList: The list of urdf files to place in the bin.

    Returns:
      The list of object unique ID's.
    """


    # Randomize positions of each object urdf.
    objectUids = []
    for urdf_name in urdfList:
      xpos = 0.4 +self._blockRandom*random.random()
      ypos = self._blockRandom*(random.random()-.5)
      angle = np.pi/2 + self._blockRandom * np.pi * random.random()
      orn = p.getQuaternionFromEuler([0, 0, angle])
      urdf_path = os.path.join(self._urdfRoot, urdf_name)
      uid = p.loadURDF(urdf_path, [xpos, ypos, .15],
        [orn[0], orn[1], orn[2], orn[3]])
      objectUids.append(uid)
      # Let each object fall to the tray individual, to prevent object
      # intersection.
      for _ in range(500):
        p.stepSimulation()
    return objectUids 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import getQuaternionFromEuler [as 別名]
def __init__(self, path, position, orientation):
        self.orientation = orientation
        self.position = position
        self.path = path
        self.plane = p.loadURDF(self.path, basePosition=self.position,
                                baseOrientation=p.getQuaternionFromEuler(self.orientation)) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import getQuaternionFromEuler [as 別名]
def setBallPos(self, x, y):
        """Sets the ball position on the field"""
        if self.ball is not None:
            # Putting the ball on the ground at given position
            p.resetBasePositionAndOrientation(
                self.ball, [x, y, 0.06], p.getQuaternionFromEuler([0, 0, 0]))

            # Stopping the ball speed
            p.changeDynamics(self.ball, 0,
                             linearDamping=0, angularDamping=0.1) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import getQuaternionFromEuler [as 別名]
def quat_from_euler(euler):
        euler = list(euler)
        quat = p.getQuaternionFromEuler(euler)
        return np.array(quat, dtype=np.float32) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import getQuaternionFromEuler [as 別名]
def mat33_from_euler(euler):
        euler = list(euler)
        quat = p.getQuaternionFromEuler(euler)
        mat33 = p.getMatrixFromQuaternion(quat)
        return np.reshape(mat33, [3, 3]) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import getQuaternionFromEuler [as 別名]
def pos_in_frame(pos, frame):
        frame_xyz = frame[0]
        frame_rpy = frame[1]
        quat = p.getQuaternionFromEuler(frame_rpy)
        mat33 = p.getMatrixFromQuaternion(quat)
        mat33 = np.reshape(mat33, [3, 3])
        pos_in_frame = frame_xyz + np.dot(pos, mat33.T)
        return pos_in_frame 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import getQuaternionFromEuler [as 別名]
def set_cstr_dof(cstr, pos, euler, max_force):
        pos = list(pos)
        euler = list(euler)
        quat = p.getQuaternionFromEuler(euler)
        p.changeConstraint(
                userConstraintUniqueId=cstr,
                jointChildPivot=pos,
                jointChildFrameOrientation=quat,
                maxForce=max_force) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import getQuaternionFromEuler [as 別名]
def step(self, action):
        self.take_step(action, robot_arm='right', gains=self.config('robot_gains'), forces=self.config('robot_forces'), human_gains=0.0005)

        robot_force_on_human, cup_force_on_human = self.get_total_force()
        total_force_on_human = robot_force_on_human + cup_force_on_human
        reward_water, water_mouth_velocities, water_hit_human_reward = self.get_water_rewards()
        end_effector_velocity = np.linalg.norm(p.getBaseVelocity(self.cup, physicsClientId=self.id)[0])
        obs = self._get_obs([cup_force_on_human], [robot_force_on_human, cup_force_on_human])

        # Get human preferences
        preferences_score = self.human_preferences(end_effector_velocity=end_effector_velocity, total_force_on_human=robot_force_on_human, tool_force_at_target=cup_force_on_human, food_hit_human_reward=water_hit_human_reward, food_mouth_velocities=water_mouth_velocities)

        cup_pos, cup_orient = p.getBasePositionAndOrientation(self.cup, physicsClientId=self.id)
        cup_pos, cup_orient = p.multiplyTransforms(cup_pos, cup_orient, [0, 0.06, 0], p.getQuaternionFromEuler([np.pi/2.0, 0, 0], physicsClientId=self.id), physicsClientId=self.id)
        cup_top_center_pos, _ = p.multiplyTransforms(cup_pos, cup_orient, self.cup_top_center_offset, [0, 0, 0, 1], physicsClientId=self.id)
        reward_distance = -np.linalg.norm(self.target_pos - np.array(cup_top_center_pos)) # Penalize distances between top of cup and mouth
        reward_action = -np.sum(np.square(action)) # Penalize actions
        # Encourage robot to have a tilted end effector / cup
        cup_euler = p.getEulerFromQuaternion(cup_orient, physicsClientId=self.id)
        reward_tilt = -abs(cup_euler[0] + np.pi/2) if self.robot_type == 'jaco' else -abs(cup_euler[0] - np.pi/2)

        reward = self.config('distance_weight')*reward_distance + self.config('action_weight')*reward_action + self.config('cup_tilt_weight')*reward_tilt + self.config('drinking_reward_weight')*reward_water + preferences_score

        if self.gui and reward_water != 0:
            print('Task success:', self.task_success, 'Water reward:', reward_water)

        info = {'total_force_on_human': total_force_on_human, 'task_success': int(self.task_success >= self.total_water_count*self.config('task_success_threshold')), 'action_robot_len': self.action_robot_len, 'action_human_len': self.action_human_len, 'obs_robot_len': self.obs_robot_len, 'obs_human_len': self.obs_human_len}
        done = False

        return obs, reward, done, info