How to use the pybullet.getLinkState function in pybullet

def state_fields_of_pose_of(self, link_id=-1): # a method you will most probably need a lot to get pose and orientation
        if link_id == -1:
            (x,y,z), (a,b,c,d) = p.getBasePositionAndOrientation(self.body_id)
        else:
            (x,y,z), (a,b,c,d),_,_,_,_ = p.getLinkState(self.body_id, link_id)
        return np.array([x,y,z,a,b,c,d])

def state_fields_of_pose_of(self, body_id, link_id=-1):  # a method you will most probably need a lot to get pose and orientation
		if link_id == -1:
			(x, y, z), (a, b, c, d) = p.getBasePositionAndOrientation(body_id)
		else:
			(x, y, z), (a, b, c, d), _, _, _, _ = p.getLinkState(body_id, link_id)
		return np.array([x, y, z, a, b, c, d])

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
                        self.reset_robot_joints()
                        # Find IK solution
                        success, joint_positions_q_star = self.util.ik_jlwki(robot, joint, target_pos, orient, self.world_creation, joint_indices[i], lower_limits[i], upper_limits[i], ik_indices=ik_indices[i], max_iterations=max_ik_iterations, success_threshold=0.03, half_range=(self.robot_type=='baxter'), step_sim=step_sim, check_env_collisions=check_env_collisions)
                        if success:
                            goal_success = True
                        else:
                            goal_success = False
                            break
                        joint_positions, _, _ = self.get_motor_joint_states(robot)
                        joint_velocities = [0.0] * len(joint_positions)
                        joint_accelerations = [0.0] * len(joint_positions)
                        center_of_mass = p.getLinkState(robot, joint, computeLinkVelocity=True, computeForwardKinematics=True, physicsClientId=self.id)[2]
                        J_linear, J_angular = p.calculateJacobian(robot, joint, localPosition=center_of_mass, objPositions=joint_positions, objVelocities=joint_velocities, objAccelerations=joint_accelerations, physicsClientId=self.id)
                        J_linear = np.array(J_linear)[:, ik_indices[i]]
                        J_angular = np.array(J_angular)[:, ik_indices[i]]
                        J = np.concatenate([J_linear, J_angular], axis=0)
                        # Joint-limited-weighting
                        joint_limit_weight = self.joint_limited_weighting(joint_positions_q_star, lower_limits[i], upper_limits[i])
                        # Joint-limited-weighted kinematic isotropy (JLWKI)
                        det = np.linalg.det(np.matmul(np.matmul(J, joint_limit_weight), J.T))
                        if det < 0:
                            det = 0
                        jlwki = np.power(det, 1.0/a) / (np.trace(np.matmul(np.matmul(J, joint_limit_weight), J.T))/a)
                        if best_jlwki is None or jlwki > best_jlwki:
                            best_jlwki = jlwki
                    if goal_success:
                        num_goals_reached += 1
                        manipulability += best_jlwki

def state_fields_of_pose_of(body_id, link_id=-1): # a method you will most probably need a lot to get pose and orientation
    if link_id == -1:
        (x,y,z), (a,b,c,d) = p.getBasePositionAndOrientation(body_id)
    else:
        (x,y,z), (a,b,c,d),_,_,_,_ = p.getLinkState(body_id, link_id)
    return np.array([x,y,z,a,b,c,d])

def _state_fields_of_pose_of(self, body_id, link_id=-1):
        """Calls native pybullet method for getting real (scaled) robot body pose

           Note that there is difference between xyz in real world scale and xyz
           in simulation. Thus you should never call pybullet methods directly
        """
        if link_id == -1:
            (x, y, z), (a, b, c, d) = p.getBasePositionAndOrientation(body_id)
        else:
            (x, y, z), (a, b, c, d), _, _, _, _ = p.getLinkState(body_id, link_id)
        x, y, z = x * self.scale, y * self.scale, z * self.scale
        return np.array([x, y, z, a, b, c, d])

def state_fields_of_pose_of(body_id, link_id=-1):
	if link_id == -1:
		(x,y,z), (a,b,c,d) = p.getBasePositionAndOrientation(body_id)
	else:
		(x,y,z), (a,b,c,d),_,_,_,_ = p.getLinkState(body_id, link_id)
	return np.array([x,y,z,a,b,c,d])

#p.changeVisualShape(dobot,4,rgbaColor=[1,1,1,1])
#p.changeVisualShape(dobot,3,rgbaColor=[0,1,0,1])
#p.changeVisualShape(dobot,6,rgbaColor=[0,1,0,1])
#p.changeVisualShape(dobot,5,rgbaColor=[1,1,0,1])

c = p.createConstraint(dobot,6,dobot,8,jointType=p.JOINT_POINT2POINT,jointAxis =[1,0,0],parentFramePosition=[-0.05,0,0.08],childFramePosition=[-0.05,0.026,-0.006])#0.014-0.02 (inertial com)

c = p.createConstraint(dobot,3,dobot,5,jointType=p.JOINT_POINT2POINT,jointAxis =[1,0,0],parentFramePosition=[-0.05,0,0.075],childFramePosition=[-0.05,-0.026,0.014])

0.150
p.getCameraImage(320,200)#160,100)
p.resetDebugVisualizerCamera(1,85.6,0,[-0.61,0.12,0.25])
p.setJointMotorControl2(dobot,6,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
for i in range (p.getNumJoints(dobot)):
	worldLinkFramePosition = p.getLinkState(dobot,i)[4]
	worldLinkFrameOrientation = p.getLinkState(dobot,i)[5]
	print(i,worldLinkFramePosition)
	
while (1):
	p.setGravity(0,0,0)
	ls = p.getLinkState(dobot,5)
	linkOrn = ls[1]
	eul = p.getEulerFromQuaternion(linkOrn)
	#print(eul)

else:
		t=t+0.01
		time.sleep(0.01)
	
	for i in range (1):
		pos = [1.0,0.2*math.cos(t),0.+0.2*math.sin(t)]
		jointPoses = p.calculateInverseKinematics(sawyerId,sawyerEndEffectorIndex,pos,jointDamping=jd)

		#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
		for i in range (numJoints):
			jointInfo = p.getJointInfo(sawyerId, i)
			qIndex = jointInfo[3]
			if qIndex > -1:
				p.resetJointState(sawyerId,i,jointPoses[qIndex-7])

	ls = p.getLinkState(sawyerId,sawyerEndEffectorIndex)
	if (hasPrevPose):
		p.addUserDebugLine(prevPose,pos,[0,0,0.3],1,trailDuration)
		p.addUserDebugLine(prevPose1,ls[4],[1,0,0],1,trailDuration)
	prevPose=pos
	prevPose1=ls[4]
	hasPrevPose = 1