def init_pybullet_robot(self):
    'load robot URDF model, set gravity, ground plane and environment'
    urdf_path = self.get_parameter('robot_urdf_path').value
    if (urdf_path == None):
        self.get_logger().warn('mandatory param robot_urdf_path not set, will exit now')
        rclpy.shutdown()
    if (not os.path.isfile(urdf_path)):
        self.get_logger().error(('param robot_urdf_path is set, but file does not exist : ' + urdf_path))
        rclpy.shutdown()
        return None
    if ('xacro' in urdf_path):
        urdf_path_without_xacro = (urdf_path[0:urdf_path.find('.xacro')] + urdf_path[(urdf_path.find('.xacro') + len('.xacro')):])
        os.system(f'xacro {urdf_path} -o {urdf_path_without_xacro}')
        urdf_path = urdf_path_without_xacro
    robot_pose_x = self.get_parameter('robot_pose_x').value
    robot_pose_y = self.get_parameter('robot_pose_y').value
    robot_pose_z = self.get_parameter('robot_pose_z').value
    robot_pose_yaw = self.get_parameter('robot_pose_yaw').value
    robot_spawn_orientation = self.pb.getQuaternionFromEuler([0.0, 0.0, robot_pose_yaw])
    fixed_base = self.get_parameter('fixed_base').value
    if self.get_parameter('use_inertia_from_file').value:
        urdf_flags = (self.pb.URDF_USE_INERTIA_FROM_FILE | self.pb.URDF_USE_SELF_COLLISION)
    else:
        urdf_flags = self.pb.URDF_USE_SELF_COLLISION
    self.get_logger().info('loading environment')
    self.environment.load_environment()
    self.pb.setRealTimeSimulation(0)
    self.get_logger().info(('loading urdf model: ' + urdf_path))
    return self.pb.loadURDF(urdf_path, basePosition=[robot_pose_x, robot_pose_y, robot_pose_z], baseOrientation=robot_spawn_orientation, useFixedBase=fixed_base, flags=urdf_flags)
load robot URDF model, set gravity, ground plane and environment
def init_pybullet_robot(self):
    
    urdf_path = self.get_parameter('robot_urdf_path').value
    if (urdf_path == None):
        self.get_logger().warn('mandatory param robot_urdf_path not set, will exit now')
        rclpy.shutdown()
    if (not os.path.isfile(urdf_path)):
        self.get_logger().error(('param robot_urdf_path is set, but file does not exist : ' + urdf_path))
        rclpy.shutdown()
        return None
    if ('xacro' in urdf_path):
        urdf_path_without_xacro = (urdf_path[0:urdf_path.find('.xacro')] + urdf_path[(urdf_path.find('.xacro') + len('.xacro')):])
        os.system(f'xacro {urdf_path} -o {urdf_path_without_xacro}')
        urdf_path = urdf_path_without_xacro
    robot_pose_x = self.get_parameter('robot_pose_x').value
    robot_pose_y = self.get_parameter('robot_pose_y').value
    robot_pose_z = self.get_parameter('robot_pose_z').value
    robot_pose_yaw = self.get_parameter('robot_pose_yaw').value
    robot_spawn_orientation = self.pb.getQuaternionFromEuler([0.0, 0.0, robot_pose_yaw])
    fixed_base = self.get_parameter('fixed_base').value
    if self.get_parameter('use_inertia_from_file').value:
        urdf_flags = (self.pb.URDF_USE_INERTIA_FROM_FILE | self.pb.URDF_USE_SELF_COLLISION)
    else:
        urdf_flags = self.pb.URDF_USE_SELF_COLLISION
    self.get_logger().info('loading environment')
    self.environment.load_environment()
    self.pb.setRealTimeSimulation(0)
    self.get_logger().info(('loading urdf model: ' + urdf_path))
    return self.pb.loadURDF(urdf_path, basePosition=[robot_pose_x, robot_pose_y, robot_pose_z], baseOrientation=robot_spawn_orientation, useFixedBase=fixed_base, flags=urdf_flags)