How to use the pybullet.getQuaternionFromEuler function in pybullet

def goToPosXY(coords):
        desiredOrientation = pybullet.getQuaternionFromEuler([0, 3.14, -1.57])
        action = pybullet.calculateInverseKinematics(0, 7, coords,
                                                     desiredOrientation,
                                                     maxNumIterations=1000,
                                                     residualThreshold=0.001)
        return action[:9]

self.human.setup_joints(joints_positions, use_static_joints=True, reactive_force=0.01)
        self.human.set_mass(self.human.base, mass=0)
        self.human.set_base_velocity(linear_velocity=[0, 0, 0], angular_velocity=[0, 0, 0])

        # Let the right arm fall to the ground
        for _ in range(100):
            p.stepSimulation(physicsClientId=self.id)

        elbow_pos = self.human.get_pos_orient(self.human.right_elbow)[0]
        wrist_pos = self.human.get_pos_orient(self.human.right_wrist)[0]
        stomach_pos = self.human.get_pos_orient(self.human.stomach)[0]
        waist_pos = self.human.get_pos_orient(self.human.waist)[0]

        target_ee_right_pos = np.array([-0.9, -0.3, 0.8]) + self.np_random.uniform(-0.05, 0.05, size=3)
        target_ee_left_pos = np.array([-0.9, 0.7, 0.8]) + self.np_random.uniform(-0.05, 0.05, size=3)
        target_ee_orient = np.array(p.getQuaternionFromEuler(np.array(self.robot.toc_ee_orient_rpy[self.task]), physicsClientId=self.id))
        # Use TOC with JLWKI to find an optimal base position for the robot near the person
        if self.robot.has_single_arm:
            target_ee_right_pos = np.array([-0.9, 0.4, 0.8]) + self.np_random.uniform(-0.05, 0.05, size=3)
            base_position, _, _ = self.robot.position_robot_toc(self.task, 'right', [(target_ee_right_pos, target_ee_orient)], [(wrist_pos, None), (waist_pos, None), (elbow_pos, None), (stomach_pos, None)], self.human, step_sim=True, check_env_collisions=False)
        else:
            base_position, _, _ = self.robot.position_robot_toc(self.task, ['right', 'left'], [[(target_ee_right_pos, target_ee_orient)], [(target_ee_left_pos, target_ee_orient)]], [[(wrist_pos, None), (waist_pos, None)], [(elbow_pos, None), (stomach_pos, None)]], self.human, step_sim=True, check_env_collisions=False)
        if self.robot.wheelchair_mounted:
            # Load a nightstand in the environment for the jaco arm
            self.nightstand = Furniture()
            self.nightstand.init('nightstand', self.directory, self.id, self.np_random)
            self.nightstand.set_base_pos_orient(np.array([-0.9, 0.7, 0]) + base_position, [np.pi/2.0, 0, 0])
        # Open gripper to hold the tools
        self.robot.set_gripper_open_position(self.robot.right_gripper_indices, self.robot.gripper_pos[self.task], set_instantly=True)
        # Initialize the tool in the robot's gripper
        self.tool_right.init(self.robot, self.task, self.directory, self.id, self.np_random, right=True, mesh_scale=[0.001]*3)
        if not self.robot.has_single_arm:

objects = [p.loadURDF("sphere_small.urdf", [4.705443,2.788506,-1.319134],[0.830352,-0.126438,-0.528531,0.123221], useMaximalCoordinates=useMaximalCoordinatesEnvObjects)]
objects = [p.loadURDF("sphere_small.urdf", [4.645979,2.714498,-1.320512],[0.837715,0.056015,-0.458430,-0.291440], useMaximalCoordinates=useMaximalCoordinatesEnvObjects)]
objects = [p.loadURDF("sphere_small.urdf", [4.661952,3.834003,-1.363581],[0.731792,-0.049574,-0.270830,0.623437], useMaximalCoordinates=useMaximalCoordinatesEnvObjects)]



#objects = p.loadSDF("stadium.sdf")

visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,fileName="marble_cube.obj", rgbaColor=[1,1,1,1], specularColor=[1,1,1], visualFramePosition=shift, meshScale=meshScale)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="marble_cube.obj", collisionFramePosition=shift,meshScale=meshScale)
#collisionShapeId = -1
uiCube = p.createMultiBody(baseMass=0,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [0,1,0], linkMasses=link_Masses,linkCollisionShapeIndices=linkCollisionShapeIndices,linkVisualShapeIndices=linkVisualShapeIndices,linkPositions=linkPositions,linkOrientations=linkOrientations,linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations,linkParentIndices=linkIndices,linkJointTypes=linkJointTypes,linkJointAxis=linkJointAxis, useMaximalCoordinates=False)

p.changeVisualShape(uiCube,0,rgbaColor=[0,0,0,1])

textOrn = p.getQuaternionFromEuler([0,0,-1.5707963])
numLines = 1
lines = [-1]*numLines

p.stepSimulation()
#disable rendering during loading makes it much faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
p.configureDebugVisualizer(p.COV_ENABLE_WIREFRAME, 0)
p.configureDebugVisualizer(p.COV_ENABLE_KEYBOARD_SHORTCUTS, 0)


#p.addUserDebugLine([0,0,0],[1,0,0],[1,0,0],lineWidth=2)
#p.addUserDebugLine([0,0,0],[0,1,0],[0,1,0],lineWidth=2)
#p.addUserDebugLine([0,0,0],[0,0,1],[0,0,1],lineWidth=2)

#objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]

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)

lower_limits = [lower_limits]
            upper_limits = [upper_limits]
            ik_indices = [ik_indices]
        a = 6 # Order of the robot space. 6D (3D position, 3D orientation)
        best_position = None
        best_orientation = None
        best_num_goals_reached = None
        best_manipulability = None
        best_start_joint_poses = [None]*len(joints)
        start_fails = 0
        iteration = 0
        best_pose_count = 0
        while iteration < attempts or best_position is None:
            iteration += 1
            random_pos = np.array([self.np_random.uniform(-random_position if right_side else 0, 0 if right_side else random_position), self.np_random.uniform(-random_position, random_position), 0])
            random_orientation = p.getQuaternionFromEuler([base_euler_orient[0], base_euler_orient[1], base_euler_orient[2] + np.deg2rad(self.np_random.uniform(-random_rotation, random_rotation))], physicsClientId=self.id)
            p.resetBasePositionAndOrientation(robot, np.array([-0.85, -0.4, 0]) + pos_offset + random_pos, random_orientation, physicsClientId=self.id)
            # Check if the robot can reach all target locations from this base pose
            num_goals_reached = 0
            manipulability = 0.0
            start_joint_poses = [None]*len(joints)
            for i, joint in enumerate(joints):
                for j, (target_pos, target_orient) in enumerate(start_pos_orient[i] + target_pos_orients[i]):
                    best_jlwki = None
                    goal_success = False
                    orient = target_orient
                    for k in range(ik_random_restarts):
                        # Reset human joints in case they got perturbed by previous iterations
                        if human_joint_positions is not None:
                            for h, pos in zip(human_joint_indices, human_joint_positions):
                                p.resetJointState(self.human, jointIndex=h, targetValue=pos, targetVelocity=0, physicsClientId=self.id)
                        # Reset all robot joints

if furniture_type == 'wheelchair':
            # Create wheelchair
            if self.robot_type in ['jaco', 'kinova_gen3']:
                furniture = p.loadURDF(os.path.join(self.directory, 'wheelchair', 'wheelchair_jaco.urdf' if self.task not in ['dressing'] else 'wheelchair_jaco_left.urdf'), physicsClientId=self.id)
            else:
                furniture = p.loadURDF(os.path.join(self.directory, 'wheelchair', 'wheelchair.urdf'), physicsClientId=self.id)
            # Initialize chair position
            p.resetBasePositionAndOrientation(furniture, [0, 0, 0.06], p.getQuaternionFromEuler([np.pi/2.0, 0, np.pi], physicsClientId=self.id), physicsClientId=self.id)
        elif furniture_type == 'bed':
            mesh_scale = [1.1]*3
            bed_visual = p.createVisualShape(shapeType=p.GEOM_MESH, fileName=os.path.join(self.directory, 'bed', 'bed_single_reduced.obj'), rgbaColor=[1, 1, 1, 1], specularColor=[0.2, 0.2, 0.2], meshScale=mesh_scale, physicsClientId=self.id)
            bed_collision = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName=os.path.join(self.directory, 'bed', 'bed_single_reduced_vhacd.obj'), meshScale=mesh_scale, flags=p.GEOM_FORCE_CONCAVE_TRIMESH, physicsClientId=self.id)
            furniture = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=bed_collision, baseVisualShapeIndex=bed_visual, basePosition=[0, 0, 0], useMaximalCoordinates=True, physicsClientId=self.id)
            # Initialize bed position
            p.resetBasePositionAndOrientation(furniture, [-0.1, 0, 0], p.getQuaternionFromEuler([np.pi/2.0, 0, 0], physicsClientId=self.id), physicsClientId=self.id)
        elif furniture_type == 'table':
            furniture = p.loadURDF(os.path.join(self.directory, 'table', 'table.urdf'), basePosition=[0, -0.35, 0], baseOrientation=p.getQuaternionFromEuler([0, 0, np.pi/2.0], physicsClientId=self.id), physicsClientId=self.id)
        else:
            furniture = None

        # Choose gender
        if gender not in ['male', 'female']:
            gender = self.np_random.choice(['male', 'female'])
        # Specify human impairments
        if human_impairment == 'random':
            human_impairment = self.np_random.choice(['none', 'limits', 'weakness', 'tremor'])
        elif human_impairment == 'no_tremor':
            human_impairment = self.np_random.choice(['none', 'limits', 'weakness'])
        self.human_impairment = human_impairment
        self.human_limit_scale = 1.0 if human_impairment != 'limits' else self.np_random.uniform(0.5, 1.0)
        self.human_strength = 1.0 if human_impairment != 'weakness' else self.np_random.uniform(0.25, 1.0)

def loadModels(self):
        p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
        p.setGravity(0, 0, -9.81)

        orn = p.getQuaternionFromEuler([math.pi/30*0, 0*math.pi/50, 0])
        p.setAdditionalSearchPath(pybullet_data.getDataPath())
        planeUid = p.loadURDF("plane_transparent.urdf", [0, 0, 0], orn)
        p.changeDynamics(planeUid, -1, lateralFriction=1)
        texUid = p.loadTexture("concrete.png")
        p.changeVisualShape(planeUid, -1, textureUniqueId=texUid)
        if self.useStairs:
            stairsUid = p.loadURDF("../urdf/stairs_gen.urdf.xml", [0, -1, 0], orn)
        flags=p.URDF_USE_SELF_COLLISION
        quadruped = p.loadURDF("../urdf/spotmicroai_gen.urdf.xml", self.init_position,
                            self.init_oritentation,
                            useFixedBase=self.useFixedBase,
                            useMaximalCoordinates=self.useMaximalCoordinates,
                            flags=flags) #p.URDF_USE_IMPLICIT_CYLINDER)
        p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
        p.changeDynamics(quadruped, -1, lateralFriction=0.8)

pybullet.setGravity(0,0,-9.81)
        pybullet.setTimeStep(self.timestep)

        # Read in configuration file
        config = yaml.load(open(self.config))

        if config["with_ground"] == True:
            self.ground_id = load_pybullet_from_urdf_or_sdf(os.environ["SPARTAN_SOURCE_DIR"] + "/build/bullet3/data/plane.urdf")
        else:
            self.ground_id = None

        # Load in the Kuka
        if config["robot"]:
            q0 = config["robot"]["base_pose"]
            position = q0[0:3]
            quaternion = pybullet.getQuaternionFromEuler(q0[3:6])
            self.kuka_id = load_pybullet_from_urdf_or_sdf(kIiwaUrdf, position, quaternion, True, self.packageMap)

            for obj_id in range(-1, pybullet.getNumJoints(self.kuka_id)):
                print "Dynamic info for body %d, %d:" % (self.kuka_id, obj_id)
                print pybullet.getDynamicsInfo(self.kuka_id, obj_id)
                pybullet.changeDynamics(self.kuka_id, obj_id, lateralFriction=0.9, spinningFriction=0.9, frictionAnchor=1)

        self.BuildJointNameInfo()
        self.BuildMotorIdList()

        # Models entry is a dictionary of model URDF strings
        model_dict = config["models"]
        self.object_ids = []

        # Add each model as requested
        self.rbts = []

def create_constraint(self, parent_link, child, child_link, joint_type=p.JOINT_FIXED, joint_axis=[0, 0, 0], parent_pos=[0, 0, 0], child_pos=[0, 0, 0], parent_orient=[0, 0, 0], child_orient=[0, 0, 0]):
        if len(parent_orient) < 4:
            parent_orient = p.getQuaternionFromEuler(parent_orient, physicsClientId=self.id)
        if len(child_orient) < 4:
            child_orient = p.getQuaternionFromEuler(child_orient, physicsClientId=self.id)
        return p.createConstraint(self.body, parent_link, child.body, child_link, joint_type, joint_axis, parent_pos, child_pos, parent_orient, child_orient, physicsClientId=self.id)

actually pick up and manipulate.
        '''

        rospack = rospkg.RosPack()
        path = rospack.get_path('costar_objects')
        sdf_dir = os.path.join(path, self.sdf_dir)
        objs = [obj for obj in os.listdir(
            sdf_dir) if os.path.isdir(os.path.join(sdf_dir, obj))]

        randn = np.random.randint(1, len(objs))

        objs_name_to_add = np.random.choice(objs, randn)
        objs_to_add = [os.path.join(sdf_dir, obj, self.model_file_name)
                       for obj in objs_name_to_add]

        identity_orientation = pb.getQuaternionFromEuler([0,0,0])
        # load sdfs for all objects and initialize positions
        for obj_index, obj in enumerate(objs_to_add):
            if objs_name_to_add[obj_index] in self.models:
                try:
                    print 'Loading object: ', obj
                    obj_id_list = pb.loadSDF(obj)
                    for obj_id in obj_id_list:
                        self.objs.append(obj_id)
                        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